JP3330872B2 - 追尾装置 - Google Patents
追尾装置Info
- Publication number
- JP3330872B2 JP3330872B2 JP09934998A JP9934998A JP3330872B2 JP 3330872 B2 JP3330872 B2 JP 3330872B2 JP 09934998 A JP09934998 A JP 09934998A JP 9934998 A JP9934998 A JP 9934998A JP 3330872 B2 JP3330872 B2 JP 3330872B2
- Authority
- JP
- Japan
- Prior art keywords
- angular velocity
- prediction
- covariance matrix
- error covariance
- value
- 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.)
- Expired - Fee Related
Links
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
【０００１】
【発明の属する技術分野】この発明は、観測雑音を含ん
だ目標位置情報を入力し、目標位置情報の真値、速度、
加速度等の目標運動諸元の推定の精度を向上した追尾装
置に関するものである。
だ目標位置情報を入力し、目標位置情報の真値、速度、
加速度等の目標運動諸元の推定の精度を向上した追尾装
置に関するものである。
【０００２】
【従来の技術】従来の追尾装置について図面を参照しな
がら説明する。図２１は、例えば特公平４−２９０３４
号公報に示された従来の追尾装置の構成を示すブロック
図である。
がら説明する。図２１は、例えば特公平４−２９０３４
号公報に示された従来の追尾装置の構成を示すブロック
図である。
【０００３】図２１において、１は観測手段、２は等角
速度運動に基づく予測手段、４は第１の遅延要素、７は
第２の遅延要素、８は等角速度運動に基づく平滑手段で
ある。また、２０は第２の角速度算出手段、２１は座標
変換行列算出手段、２２は駆動手段である。
速度運動に基づく予測手段、４は第１の遅延要素、７は
第２の遅延要素、８は等角速度運動に基づく平滑手段で
ある。また、２０は第２の角速度算出手段、２１は座標
変換行列算出手段、２２は駆動手段である。
【０００４】観測手段１は、極座標において目標位置情
報を観測し、その目標位置情報を固定直交座標に変換し
て出力する。第２の角速度算出手段２０は、目標の位置
推移に無関係な速度ベクトル方向の角速度を０、目標の
速度ベクトルを回転させる角速度を一定、目標進行方向
の加速度を一定とするという前提条件を用いて、角速度
を算出する。
報を観測し、その目標位置情報を固定直交座標に変換し
て出力する。第２の角速度算出手段２０は、目標の位置
推移に無関係な速度ベクトル方向の角速度を０、目標の
速度ベクトルを回転させる角速度を一定、目標進行方向
の加速度を一定とするという前提条件を用いて、角速度
を算出する。
【０００５】等角速度運動に基づく予測手段２は、現時
刻より１サンプリング前の上記第２の角速度算出手段２
０より算出される角速度より状態ベクトルの状態遷移行
列、および現時刻より１サンプリング後の目標の運動諸
元の予測値、予測誤差共分散行列を算出する。
刻より１サンプリング前の上記第２の角速度算出手段２
０より算出される角速度より状態ベクトルの状態遷移行
列、および現時刻より１サンプリング後の目標の運動諸
元の予測値、予測誤差共分散行列を算出する。
【０００６】等角速度運動に基づく平滑手段８は、目標
の位置・速度・加速度などの目標運動諸元の平滑値を等
角速度運動に基づく状態遷移行列を用いて算出する。座
標変換行列算出手段２１は、上記等角速度運動に基づく
予測手段２から算出された予測値を用いて、固定直交座
標から動的直交座標への変換行列を算出する。駆動装置
２２は、上記観測手段１を目標に指向させる。
の位置・速度・加速度などの目標運動諸元の平滑値を等
角速度運動に基づく状態遷移行列を用いて算出する。座
標変換行列算出手段２１は、上記等角速度運動に基づく
予測手段２から算出された予測値を用いて、固定直交座
標から動的直交座標への変換行列を算出する。駆動装置
２２は、上記観測手段１を目標に指向させる。
【０００７】従来の追尾装置は上記のように構成され、
例えば座標系としては、目標の速度ベクトルを１軸とし
た動的直交座標が使用され、角速度算出を行なう前提条
件としては、目標進行方向の加速度を一定とすること
で、角速度を算出している。
例えば座標系としては、目標の速度ベクトルを１軸とし
た動的直交座標が使用され、角速度算出を行なう前提条
件としては、目標進行方向の加速度を一定とすること
で、角速度を算出している。
【０００８】すなわち、観測手段１では、極座標で表さ
れる観測雑音を含む目標位置情報として距離、仰角、方
位角を観測する。第２の角速度算出手段２０では、角速
度の算出を行なう時、等角速度運動に基づく平滑手段８
で算出した平滑値を入力し、目標の位置推移に無関係な
速度ベクトル方向の角速度を０、目標の速度ベクトルを
回転させる角速度を一定、目標進行方向の加速度を一定
とするという前提条件を用いて角速度を算出する。
れる観測雑音を含む目標位置情報として距離、仰角、方
位角を観測する。第２の角速度算出手段２０では、角速
度の算出を行なう時、等角速度運動に基づく平滑手段８
で算出した平滑値を入力し、目標の位置推移に無関係な
速度ベクトル方向の角速度を０、目標の速度ベクトルを
回転させる角速度を一定、目標進行方向の加速度を一定
とするという前提条件を用いて角速度を算出する。
【０００９】また、等角速度運動に基づく予測手段２で
は、第１の遅延要素４を通して入力される第２の角速度
算出手段２０より算出される角速度をを用いて、状態ベ
クトルの状態遷移行列および現時刻より１サンプリング
後の目標の運動諸元の予測値、予測誤差共分散行列を算
出する。
は、第１の遅延要素４を通して入力される第２の角速度
算出手段２０より算出される角速度をを用いて、状態ベ
クトルの状態遷移行列および現時刻より１サンプリング
後の目標の運動諸元の予測値、予測誤差共分散行列を算
出する。
【００１０】また、等角速度運動に基づく平滑手段８で
は、座標変換行列算出手段２１で算出された座標変換行
列を用いて、等角速度運動に基づく予測手段２で算出さ
れた状態遷移行列、予測値、予測誤差共分散行列を座標
変換したものを第２の遅延要素７を通して入力し、目標
の位置・速度・加速度などの目標運動諸元の平滑値を算
出する。
は、座標変換行列算出手段２１で算出された座標変換行
列を用いて、等角速度運動に基づく予測手段２で算出さ
れた状態遷移行列、予測値、予測誤差共分散行列を座標
変換したものを第２の遅延要素７を通して入力し、目標
の位置・速度・加速度などの目標運動諸元の平滑値を算
出する。
【００１１】そして、座標変換行列算出手段２１、等角
速度運動に基づく予測手段２から算出された予測値を用
いて、固定直交座標から動的直交座標への変換行列を算
出する。駆動手段２２では、等角速度運動に基づく予測
手段２から算出された予測値の位置情報を元に、現在時
刻から１サンプリング後に、観測手段１が指向すべき仰
角、方位角を算出し、その方向に観測手段１が指向する
ように、観測手段１を制御する。
速度運動に基づく予測手段２から算出された予測値を用
いて、固定直交座標から動的直交座標への変換行列を算
出する。駆動手段２２では、等角速度運動に基づく予測
手段２から算出された予測値の位置情報を元に、現在時
刻から１サンプリング後に、観測手段１が指向すべき仰
角、方位角を算出し、その方向に観測手段１が指向する
ように、観測手段１を制御する。
【００１２】
【発明が解決しようとする課題】上述したような従来の
追尾装置では、旋回目標及び直進目標に対して精度良く
追尾できないという問題点があった。
追尾装置では、旋回目標及び直進目標に対して精度良く
追尾できないという問題点があった。
【００１３】さらに、固定直交座標−動的直交座標間で
座標変換が必要であり、アルゴリズムが冗長であるとい
う問題点があった。
座標変換が必要であり、アルゴリズムが冗長であるとい
う問題点があった。
【００１４】この発明は、前述した問題点を解決するた
めになされたもので、旋回目標及び直進目標に対しても
精度良く追尾でき、固定直交座標−動的直交座標間で座
標変換を不要としてアルゴリズムを簡略化することがで
きる追尾装置を得ることを目的とする。
めになされたもので、旋回目標及び直進目標に対しても
精度良く追尾でき、固定直交座標−動的直交座標間で座
標変換を不要としてアルゴリズムを簡略化することがで
きる追尾装置を得ることを目的とする。
【００１５】
【課題を解決するための手段】この発明に係る追尾装置
は、極座標において目標位置情報を観測し、その目標位
置情報を固定直交座標に変換し観測値として出力する観
測手段と、固定直交座標において、現時刻より１サンプ
リング後の等角速度運動に基づく状態ベクトルの状態遷
移行列、並びに現時刻より１サンプリング後の目標の運
動諸元の第１の予測値、及び第１の予測誤差共分散行列
を算出する等角速度運動に基づく予測手段と、目標の位
置推移に無関係な速度ベクトル方向の角速度を０、目標
の速度ベクトルを回転させる角速度を一定、及び目標進
行方向の速度を一定とするという前提条件を用いて、目
標の速度・加速度より、固定直交座標において角速度を
算出し、さらに、目標位置ベクトルの３次微分を角速度
ベクトルの大きさと目標位置ベクトルの１次微分から算
出する角速度算出手段と、前記等角速度運動に基づく予
測手段で算出する前記状態遷移行列を算出するために、
１サンプリング前に、前記角速度算出手段で算出した角
速度を入力する第１の遅延要素と、固定直交座標におい
て、現時刻より１サンプリング後の目標の運動諸元の第
２の予測値、及び第２の予測誤差共分散行列を算出する
等速直線運動に基づく予測手段と、固定直交座標におい
て、前記等角速度運動に基づく予測手段により得られた
第１の予測値、及び第１の予測誤差共分散行列、並びに
前記等速直線運動に基づく予測手段により得られた第２
の予測値、及び第２の予測誤差共分散行列の内、前記第
１の予測値又は前記第２の予測値と前記観測手段によっ
て得られる前記観測値との残差が最小になるものを１サ
ンプリング後の第３の予測値、及び第３の予測誤差共分
散行列とする残差による予測値選択手段と、１サンプリ
ング前に、前記残差による予測値選択手段によって得ら
れた前記第３の予測値、及び第３の予測誤差共分散行列
を出力する第２の遅延要素と、固定直交座標において、
前記第２の遅延要素より得られる第３の予測値、及び第
３の予測誤差共分散行列、並びに前記観測手段により得
られる観測値より、第１の平滑値、及び第１の平滑誤差
共分散行列を算出する等角速度運動に基づく平滑手段
と、固定直交座標において、前記第２の遅延要素より得
られる第３の予測値、及び第３の予測誤差共分散行列、
並びに前記観測手段により得られる観測値より、第２の
平滑値、及び第２の平滑誤差共分散行列を算出する等速
直線運動に基づく平滑手段とを備えたものである。
は、極座標において目標位置情報を観測し、その目標位
置情報を固定直交座標に変換し観測値として出力する観
測手段と、固定直交座標において、現時刻より１サンプ
リング後の等角速度運動に基づく状態ベクトルの状態遷
移行列、並びに現時刻より１サンプリング後の目標の運
動諸元の第１の予測値、及び第１の予測誤差共分散行列
を算出する等角速度運動に基づく予測手段と、目標の位
置推移に無関係な速度ベクトル方向の角速度を０、目標
の速度ベクトルを回転させる角速度を一定、及び目標進
行方向の速度を一定とするという前提条件を用いて、目
標の速度・加速度より、固定直交座標において角速度を
算出し、さらに、目標位置ベクトルの３次微分を角速度
ベクトルの大きさと目標位置ベクトルの１次微分から算
出する角速度算出手段と、前記等角速度運動に基づく予
測手段で算出する前記状態遷移行列を算出するために、
１サンプリング前に、前記角速度算出手段で算出した角
速度を入力する第１の遅延要素と、固定直交座標におい
て、現時刻より１サンプリング後の目標の運動諸元の第
２の予測値、及び第２の予測誤差共分散行列を算出する
等速直線運動に基づく予測手段と、固定直交座標におい
て、前記等角速度運動に基づく予測手段により得られた
第１の予測値、及び第１の予測誤差共分散行列、並びに
前記等速直線運動に基づく予測手段により得られた第２
の予測値、及び第２の予測誤差共分散行列の内、前記第
１の予測値又は前記第２の予測値と前記観測手段によっ
て得られる前記観測値との残差が最小になるものを１サ
ンプリング後の第３の予測値、及び第３の予測誤差共分
散行列とする残差による予測値選択手段と、１サンプリ
ング前に、前記残差による予測値選択手段によって得ら
れた前記第３の予測値、及び第３の予測誤差共分散行列
を出力する第２の遅延要素と、固定直交座標において、
前記第２の遅延要素より得られる第３の予測値、及び第
３の予測誤差共分散行列、並びに前記観測手段により得
られる観測値より、第１の平滑値、及び第１の平滑誤差
共分散行列を算出する等角速度運動に基づく平滑手段
と、固定直交座標において、前記第２の遅延要素より得
られる第３の予測値、及び第３の予測誤差共分散行列、
並びに前記観測手段により得られる観測値より、第２の
平滑値、及び第２の平滑誤差共分散行列を算出する等速
直線運動に基づく平滑手段とを備えたものである。
【００１６】また、この発明に係る追尾装置は、さら
に、前記等角速度運動に基づく予測手段より得られる第
１の予測値、及び第１の予測誤差共分散行列から算出さ
れる第１の目標存在期待領域、前記等速直線運動に基づ
く予測手段で算出される予測値ベクトル、及び第２の予
測誤差共分散行列から算出される第２の目標存在期待領
域の内、目標位置に関する予測誤差共分散行列が最大と
なる第３の目標存在期待領域を選択する目標存在期待領
域選択手段と、第３の遅延要素を通して入力された前記
目標存在期待領域選択手段により選択された前記第３の
目標存在期待領域を用いて、前記観測手段により求めら
れる前記観測値が前記第３の目標存在期待領域内に存在
するか否かを判定する目標位置情報評価手段と、前記第
３の目標存在期待領域内に存在する観測値の内、前記第
３の目標存在期待領域内の中心からの距離に応じて算出
される確率密度により信頼度を算出する信頼度算出手段
とを備え、前記等角速度運動に基づく平滑手段は、前記
信頼度により、前記第３の予測値、及び前記第３の予測
誤差共分散行列、並びに前記観測手段により得られる観
測値の重み付けを行い、第１の平滑値、及び第１の平滑
誤差共分散行列を算出し、前記等速直線運動に基づく平
滑手段は、前記信頼度により、前記第３の予測値、及び
前記第３の予測誤差共分散行列、並びに前記観測手段に
より得られる観測値の重み付けを行い、第２の平滑値、
及び第２の平滑誤差共分散行列を算出するものである。
に、前記等角速度運動に基づく予測手段より得られる第
１の予測値、及び第１の予測誤差共分散行列から算出さ
れる第１の目標存在期待領域、前記等速直線運動に基づ
く予測手段で算出される予測値ベクトル、及び第２の予
測誤差共分散行列から算出される第２の目標存在期待領
域の内、目標位置に関する予測誤差共分散行列が最大と
なる第３の目標存在期待領域を選択する目標存在期待領
域選択手段と、第３の遅延要素を通して入力された前記
目標存在期待領域選択手段により選択された前記第３の
目標存在期待領域を用いて、前記観測手段により求めら
れる前記観測値が前記第３の目標存在期待領域内に存在
するか否かを判定する目標位置情報評価手段と、前記第
３の目標存在期待領域内に存在する観測値の内、前記第
３の目標存在期待領域内の中心からの距離に応じて算出
される確率密度により信頼度を算出する信頼度算出手段
とを備え、前記等角速度運動に基づく平滑手段は、前記
信頼度により、前記第３の予測値、及び前記第３の予測
誤差共分散行列、並びに前記観測手段により得られる観
測値の重み付けを行い、第１の平滑値、及び第１の平滑
誤差共分散行列を算出し、前記等速直線運動に基づく平
滑手段は、前記信頼度により、前記第３の予測値、及び
前記第３の予測誤差共分散行列、並びに前記観測手段に
より得られる観測値の重み付けを行い、第２の平滑値、
及び第２の平滑誤差共分散行列を算出するものである。
【００１７】また、この発明に係る追尾装置は、さら
に、前記等角速度運動に基づく予測手段から得られる第
１の予測値、及び第１の予測誤差共分散行列、前記等速
直線運動に基づく予測手段から得られる第２の予測値、
及び第２の予測誤差共分散行列、並びに前記観測手段か
ら求まる観測値を元に、前記第１又は第２の予測値と前
記観測値の残差で、目標の運動状態を判定してサンプリ
ングレートを変える残差によるサンプリング制御手段を
備えたものである。
に、前記等角速度運動に基づく予測手段から得られる第
１の予測値、及び第１の予測誤差共分散行列、前記等速
直線運動に基づく予測手段から得られる第２の予測値、
及び第２の予測誤差共分散行列、並びに前記観測手段か
ら求まる観測値を元に、前記第１又は第２の予測値と前
記観測値の残差で、目標の運動状態を判定してサンプリ
ングレートを変える残差によるサンプリング制御手段を
備えたものである。
【００１８】また、この発明に係る追尾装置は、さら
に、前記角速度算出手段より求まる角速度により目標の
運動状態を判定してサンプリングレートを変える角速度
によるサンプリング制御手段を備えたものである。
に、前記角速度算出手段より求まる角速度により目標の
運動状態を判定してサンプリングレートを変える角速度
によるサンプリング制御手段を備えたものである。
【００１９】また、この発明に係る追尾装置は、さら
に、前記等角速度運動に基づく予測手段から得られる第
１の予測値、前記等速直線運動に基づく予測手段から得
られる第２の予測値から、目標軌跡に一番近いと考えら
れる前記第１又は第２の予測位置の１つのみを表示する
表示手段を備えたものである。
に、前記等角速度運動に基づく予測手段から得られる第
１の予測値、前記等速直線運動に基づく予測手段から得
られる第２の予測値から、目標軌跡に一番近いと考えら
れる前記第１又は第２の予測位置の１つのみを表示する
表示手段を備えたものである。
【００２０】この発明に係る追尾装置は、極座標におい
て目標位置情報を観測し、その目標位置情報を固定直交
座標に変換し観測値として出力する観測手段と、固定直
交座標において、現時刻より１サンプリング後の等角速
度運動に基づく状態ベクトルの状態遷移行列、並びに現
時刻より１サンプリング後の目標の運動諸元の第１の予
測値、及び第１の予測誤差共分散行列を算出する等角速
度運動に基づく予測手段と、目標の位置推移に無関係な
速度ベクトル方向の角速度を０、目標の速度ベクトルを
回転させる角速度を一定、及び目標進行方向の速度を一
定とするという前提条件を用いて、目標の速度・加速度
より、固定直交座標において角速度を算出し、さらに、
目標位置ベクトルの３次微分を角速度ベクトルの大きさ
と目標位置ベクトルの１次微分から算出する角速度算出
手段と、前記等角速度運動に基づく予測手段で算出する
前記状態遷移行列を算出するために、１サンプリング前
に、前記角速度算出手段で算出した角速度を入力する第
１の遅延要素と、固定直交座標において、現時刻より１
サンプリング後の目標の運動諸元の第２の予測値、及び
第２の予測誤差共分散行列を算出する等速直線運動に基
づく予測手段と、固定直交座標において、前記等角速度
運動に基づく予測手段により得られた第１の予測値、及
び第１の予測誤差共分散行列、前記等速直線運動に基づ
く予測手段により得られた第２の予測値、及び第２の予
測誤差共分散行列、並びに前記観測手段により得られた
観測値を元に、前記第１又は第２の予測値と前記観測値
の残差で、前記等角速度運動に基づく第１の予測値、及
び第１の予測誤差共分散行列と前記等速直線運動に基づ
く第２の予測値、及び第２の予測誤差共分散行列を重み
付けしたものを第３の予測値、及び第３の予測誤差共分
散行列とする残差による予測値統合手段と、１サンプリ
ング前に、前記残差による予測値統合手段によって得ら
れた前記第３の予測値、及び第３の予測誤差共分散行列
を出力する第２の遅延要素と、固定直交座標において、
前記第２の遅延要素より得られる第３の予測値、及び第
３の予測誤差共分散行列、並びに前記観測手段により得
られる観測値より、第１の平滑値、及び第１の平滑誤差
共分散行列を算出する等角速度運動に基づく平滑手段
と、固定直交座標において、前記第２の遅延要素より得
られる第３の予測値、及び第３の予測誤差共分散行列、
並びに前記観測手段により得られる観測値より、第２の
平滑値、及び第２の平滑誤差共分散行列を算出する等速
直線運動に基づく平滑手段とを備えたものである。
て目標位置情報を観測し、その目標位置情報を固定直交
座標に変換し観測値として出力する観測手段と、固定直
交座標において、現時刻より１サンプリング後の等角速
度運動に基づく状態ベクトルの状態遷移行列、並びに現
時刻より１サンプリング後の目標の運動諸元の第１の予
測値、及び第１の予測誤差共分散行列を算出する等角速
度運動に基づく予測手段と、目標の位置推移に無関係な
速度ベクトル方向の角速度を０、目標の速度ベクトルを
回転させる角速度を一定、及び目標進行方向の速度を一
定とするという前提条件を用いて、目標の速度・加速度
より、固定直交座標において角速度を算出し、さらに、
目標位置ベクトルの３次微分を角速度ベクトルの大きさ
と目標位置ベクトルの１次微分から算出する角速度算出
手段と、前記等角速度運動に基づく予測手段で算出する
前記状態遷移行列を算出するために、１サンプリング前
に、前記角速度算出手段で算出した角速度を入力する第
１の遅延要素と、固定直交座標において、現時刻より１
サンプリング後の目標の運動諸元の第２の予測値、及び
第２の予測誤差共分散行列を算出する等速直線運動に基
づく予測手段と、固定直交座標において、前記等角速度
運動に基づく予測手段により得られた第１の予測値、及
び第１の予測誤差共分散行列、前記等速直線運動に基づ
く予測手段により得られた第２の予測値、及び第２の予
測誤差共分散行列、並びに前記観測手段により得られた
観測値を元に、前記第１又は第２の予測値と前記観測値
の残差で、前記等角速度運動に基づく第１の予測値、及
び第１の予測誤差共分散行列と前記等速直線運動に基づ
く第２の予測値、及び第２の予測誤差共分散行列を重み
付けしたものを第３の予測値、及び第３の予測誤差共分
散行列とする残差による予測値統合手段と、１サンプリ
ング前に、前記残差による予測値統合手段によって得ら
れた前記第３の予測値、及び第３の予測誤差共分散行列
を出力する第２の遅延要素と、固定直交座標において、
前記第２の遅延要素より得られる第３の予測値、及び第
３の予測誤差共分散行列、並びに前記観測手段により得
られる観測値より、第１の平滑値、及び第１の平滑誤差
共分散行列を算出する等角速度運動に基づく平滑手段
と、固定直交座標において、前記第２の遅延要素より得
られる第３の予測値、及び第３の予測誤差共分散行列、
並びに前記観測手段により得られる観測値より、第２の
平滑値、及び第２の平滑誤差共分散行列を算出する等速
直線運動に基づく平滑手段とを備えたものである。
【００２１】また、この発明に係る追尾装置は、さら
に、前記等角速度運動に基づく予測手段より得られる第
１の予測値、及び第１の予測誤差共分散行列から算出さ
れる第１の目標存在期待領域、前記等速直線運動に基づ
く予測手段で算出される第２の予測値、及び第２の予測
誤差共分散行列から算出される第２の目標存在期待領域
の内、前記等速直線運動に基づく目標位置に関する第２
の予測誤差共分散行列と前記等角速度運動に基づく目標
位置に関する第１の予測誤差共分散行列を前記第１又は
第２の予測値と観測値の残差で重み付けをして統合を行
ない、その統合された第３の予測誤差共分散行列を用い
て、統合された第３の目標存在期待領域を算出する目標
存在期待領域統合手段と、第３の遅延要素を通して入力
された前記目標存在期待領域統合手段により統合された
前記第３の目標存在期待領域を用いて、前記観測手段に
より求められる前記観測値が前記第３の目標存在期待領
域内に存在するか否かを判定する目標位置情報評価手段
と、前記第３の目標存在期待領域内に存在する観測値の
内、前記第３の目標存在期待領域内の中心からの距離に
応じて算出される確率密度により信頼度を算出する信頼
度算出手段とを備え、前記等角速度運動に基づく平滑手
段は、前記信頼度により、前記第３の予測値、及び前記
第３の予測誤差共分散行列、並びに前記観測手段により
得られる観測値の重み付けを行い、第１の平滑値、及び
第１の平滑誤差共分散行列を算出し、前記等速直線運動
に基づく平滑手段は、前記信頼度により、前記第３の予
測値、及び前記第３の予測誤差共分散行列、並びに前記
観測手段により得られる観測値の重み付けを行い、第２
の平滑値、及び第２の平滑誤差共分散行列を算出するも
のである。
に、前記等角速度運動に基づく予測手段より得られる第
１の予測値、及び第１の予測誤差共分散行列から算出さ
れる第１の目標存在期待領域、前記等速直線運動に基づ
く予測手段で算出される第２の予測値、及び第２の予測
誤差共分散行列から算出される第２の目標存在期待領域
の内、前記等速直線運動に基づく目標位置に関する第２
の予測誤差共分散行列と前記等角速度運動に基づく目標
位置に関する第１の予測誤差共分散行列を前記第１又は
第２の予測値と観測値の残差で重み付けをして統合を行
ない、その統合された第３の予測誤差共分散行列を用い
て、統合された第３の目標存在期待領域を算出する目標
存在期待領域統合手段と、第３の遅延要素を通して入力
された前記目標存在期待領域統合手段により統合された
前記第３の目標存在期待領域を用いて、前記観測手段に
より求められる前記観測値が前記第３の目標存在期待領
域内に存在するか否かを判定する目標位置情報評価手段
と、前記第３の目標存在期待領域内に存在する観測値の
内、前記第３の目標存在期待領域内の中心からの距離に
応じて算出される確率密度により信頼度を算出する信頼
度算出手段とを備え、前記等角速度運動に基づく平滑手
段は、前記信頼度により、前記第３の予測値、及び前記
第３の予測誤差共分散行列、並びに前記観測手段により
得られる観測値の重み付けを行い、第１の平滑値、及び
第１の平滑誤差共分散行列を算出し、前記等速直線運動
に基づく平滑手段は、前記信頼度により、前記第３の予
測値、及び前記第３の予測誤差共分散行列、並びに前記
観測手段により得られる観測値の重み付けを行い、第２
の平滑値、及び第２の平滑誤差共分散行列を算出するも
のである。
【００２２】この発明に係る追尾装置は、極座標におい
て目標位置情報を観測し、その目標位置情報を固定直交
座標に変換し観測値として出力する観測手段と、固定直
交座標において、現時刻より１サンプリング後の等角速
度運動に基づく状態ベクトルの状態遷移行列、並びに現
時刻より１サンプリング後の目標の運動諸元の第１の予
測値、及び第１の予測誤差共分散行列を算出する等角速
度運動に基づく予測手段と、目標の位置推移に無関係な
速度ベクトル方向の角速度を０、目標の速度ベクトルを
回転させる角速度を一定、及び目標進行方向の速度を一
定とするという前提条件を用いて、目標の速度・加速度
より、固定直交座標において角速度を算出し、さらに、
目標位置ベクトルの３次微分を角速度ベクトルの大きさ
と目標位置ベクトルの１次微分から算出する角速度算出
手段と、前記等角速度運動に基づく予測手段で算出する
前記状態遷移行列を算出するために、１サンプリング前
に、前記角速度算出手段で算出した角速度を入力する第
１の遅延要素と、固定直交座標において、現時刻より１
サンプリング後の目標の運動諸元の第２の予測値、及び
第２の予測誤差共分散行列を算出する等速直線運動に基
づく予測手段と、固定直交座標において、前記等角速度
運動に基づく予測手段により得られた第１の予測値、及
び第１の予測誤差共分散行列、並びに前記等速直線運動
に基づく予測手段により得られた第２の予測値、及び第
２の予測誤差共分散行列を、前記角速度算出手段より求
まる角速度の大きさにより選択する角速度による予測値
選択手段と、１サンプリング前に、前記角速度による予
測値選択手段によって得られた第３の予測値、及び第３
の予測誤差共分散行列を出力する第２の遅延要素と、固
定直交座標において、前記第２の遅延要素より得られる
第３の予測値、及び第３の予測誤差共分散行列、並びに
前記観測手段により得られる観測値より、第１の平滑
値、及び第１の平滑誤差共分散行列を算出する等角速度
運動に基づく平滑手段と、固定直交座標において、前記
第２の遅延要素より得られる第３の予測値、及び第３の
予測誤差共分散行列、並びに前記観測手段により得られ
る観測値より、第２の平滑値、及び第２の平滑誤差共分
散行列を算出する等速直線運動に基づく平滑手段とを備
えたものである。
て目標位置情報を観測し、その目標位置情報を固定直交
座標に変換し観測値として出力する観測手段と、固定直
交座標において、現時刻より１サンプリング後の等角速
度運動に基づく状態ベクトルの状態遷移行列、並びに現
時刻より１サンプリング後の目標の運動諸元の第１の予
測値、及び第１の予測誤差共分散行列を算出する等角速
度運動に基づく予測手段と、目標の位置推移に無関係な
速度ベクトル方向の角速度を０、目標の速度ベクトルを
回転させる角速度を一定、及び目標進行方向の速度を一
定とするという前提条件を用いて、目標の速度・加速度
より、固定直交座標において角速度を算出し、さらに、
目標位置ベクトルの３次微分を角速度ベクトルの大きさ
と目標位置ベクトルの１次微分から算出する角速度算出
手段と、前記等角速度運動に基づく予測手段で算出する
前記状態遷移行列を算出するために、１サンプリング前
に、前記角速度算出手段で算出した角速度を入力する第
１の遅延要素と、固定直交座標において、現時刻より１
サンプリング後の目標の運動諸元の第２の予測値、及び
第２の予測誤差共分散行列を算出する等速直線運動に基
づく予測手段と、固定直交座標において、前記等角速度
運動に基づく予測手段により得られた第１の予測値、及
び第１の予測誤差共分散行列、並びに前記等速直線運動
に基づく予測手段により得られた第２の予測値、及び第
２の予測誤差共分散行列を、前記角速度算出手段より求
まる角速度の大きさにより選択する角速度による予測値
選択手段と、１サンプリング前に、前記角速度による予
測値選択手段によって得られた第３の予測値、及び第３
の予測誤差共分散行列を出力する第２の遅延要素と、固
定直交座標において、前記第２の遅延要素より得られる
第３の予測値、及び第３の予測誤差共分散行列、並びに
前記観測手段により得られる観測値より、第１の平滑
値、及び第１の平滑誤差共分散行列を算出する等角速度
運動に基づく平滑手段と、固定直交座標において、前記
第２の遅延要素より得られる第３の予測値、及び第３の
予測誤差共分散行列、並びに前記観測手段により得られ
る観測値より、第２の平滑値、及び第２の平滑誤差共分
散行列を算出する等速直線運動に基づく平滑手段とを備
えたものである。
【００２３】
【発明の実施の形態】実施の形態１．この発明の実施の
形態１に係る追尾装置について図面を参照しながら説明
する。図１は、この発明の実施の形態１に係る追尾装置
の構成を示すブロック図である。なお、各図中、同一符
号は同一又は相当部分を示す。
形態１に係る追尾装置について図面を参照しながら説明
する。図１は、この発明の実施の形態１に係る追尾装置
の構成を示すブロック図である。なお、各図中、同一符
号は同一又は相当部分を示す。
【００２４】図１において、１は観測手段、２は等角速
度運動に基づく予測手段、３は第１の角速度算出手段、
４は第１の遅延要素、５は等速直線運動に基づく予測手
段、６は残差による予測値選択手段、７は第２の遅延要
素、８は等角速度運動に基づく平滑手段、９は等速直線
運動に基づく平滑手段である。
度運動に基づく予測手段、３は第１の角速度算出手段、
４は第１の遅延要素、５は等速直線運動に基づく予測手
段、６は残差による予測値選択手段、７は第２の遅延要
素、８は等角速度運動に基づく平滑手段、９は等速直線
運動に基づく平滑手段である。
【００２５】観測手段１は、極座標において目標位置情
報を観測し、その目標位置情報を固定直交座標に変換し
て出力する。
報を観測し、その目標位置情報を固定直交座標に変換し
て出力する。
【００２６】等角速度運動に基づく予測手段２は、固定
直交座標において、現時刻より１サンプリング後の等角
速度運動に基づく状態ベクトルの状態遷移行列および現
時刻より１サンプリング後の目標の運動諸元の予測値、
予測誤差共分散行列を算出する。
直交座標において、現時刻より１サンプリング後の等角
速度運動に基づく状態ベクトルの状態遷移行列および現
時刻より１サンプリング後の目標の運動諸元の予測値、
予測誤差共分散行列を算出する。
【００２７】第１の角速度算出手段３は、目標の位置推
移に無関係な速度ベクトル方向の角速度を０、目標の速
度ベクトルを回転させる角速度を一定、目標進行方向の
速度を一定とするという前提条件を用いて、目標の速度
・加速度より、固定直交座標において角速度を算出し、
さらに、目標位置ベクトルの３次微分を角速度ベクトル
の大きさと目標位置ベクトルの１次微分から算出する。
移に無関係な速度ベクトル方向の角速度を０、目標の速
度ベクトルを回転させる角速度を一定、目標進行方向の
速度を一定とするという前提条件を用いて、目標の速度
・加速度より、固定直交座標において角速度を算出し、
さらに、目標位置ベクトルの３次微分を角速度ベクトル
の大きさと目標位置ベクトルの１次微分から算出する。
【００２８】第１の遅延要素４は、上記等角速度運動に
基づく予測手段２で状態遷移行列を算出するために、１
サンプリング前に、上記第１の角速度算出手段３で算出
した角速度を入力する。
基づく予測手段２で状態遷移行列を算出するために、１
サンプリング前に、上記第１の角速度算出手段３で算出
した角速度を入力する。
【００２９】等速直線運動に基づく予測手段５は、固定
直交座標において、現時刻より１サンプリング後の目標
の運動諸元の予測値、予測誤差共分散行列を算出する。
直交座標において、現時刻より１サンプリング後の目標
の運動諸元の予測値、予測誤差共分散行列を算出する。
【００３０】残差による予測値選択手段６は、固定直交
座標において、上記等角速度運動に基づく予測手段２に
より得られた予測値、及び上記等速直線運動に基づく予
測手段５により得られた予測値の内、上記観測手段１に
よって得られる観測値との残差が最小になるものの１つ
を１サンプリング後の予測値とする。
座標において、上記等角速度運動に基づく予測手段２に
より得られた予測値、及び上記等速直線運動に基づく予
測手段５により得られた予測値の内、上記観測手段１に
よって得られる観測値との残差が最小になるものの１つ
を１サンプリング後の予測値とする。
【００３１】第２の遅延要素７は、１サンプリング前
に、上記等角速度運動に基づく予測手段２により得られ
る予測誤差共分散行列を等角速度運動に基づく平滑手段
８に入力し、１サンプリング前に、上記等速直線運動に
基づく予測手段５により得られる予測誤差共分散行列を
等速直線運動に基づく平滑手段９に入力し、さらに１サ
ンプリング前に、上記予測値選択手段６によって得られ
た予測値を等角速度運動に基づく平滑手段８および等速
直線運動に基づく平滑手段９に入力する。
に、上記等角速度運動に基づく予測手段２により得られ
る予測誤差共分散行列を等角速度運動に基づく平滑手段
８に入力し、１サンプリング前に、上記等速直線運動に
基づく予測手段５により得られる予測誤差共分散行列を
等速直線運動に基づく平滑手段９に入力し、さらに１サ
ンプリング前に、上記予測値選択手段６によって得られ
た予測値を等角速度運動に基づく平滑手段８および等速
直線運動に基づく平滑手段９に入力する。
【００３２】等角速度運動に基づく平滑手段８は、上記
第２の遅延要素７より得られる予測値、予測誤差共分散
行列および上記観測手段１により得られる観測値より、
平滑値、平滑誤差共分散行列を算出する。
第２の遅延要素７より得られる予測値、予測誤差共分散
行列および上記観測手段１により得られる観測値より、
平滑値、平滑誤差共分散行列を算出する。
【００３３】等速直線運動に基づく平滑手段９は、上記
第２の遅延要素７より得られる予測値、予測誤差共分散
行列および上記観測手段１により得られる観測値より、
平滑値、平滑誤差共分散行列を算出する。
第２の遅延要素７より得られる予測値、予測誤差共分散
行列および上記観測手段１により得られる観測値より、
平滑値、平滑誤差共分散行列を算出する。
【００３４】次に、第１の角速度算出手段３を説明する
ために必要な座標系を以下に説明する。
ために必要な座標系を以下に説明する。
【００３５】図２は、観測装置を原点Ｏ、東方向をｘ軸
の正Ｘ、北方向をｙ軸の正Ｙ、鉛直上方向をｚ軸の正Ｚ
とする固定直交座標Ｏ−ｘｙｚを示す図である。
の正Ｘ、北方向をｙ軸の正Ｙ、鉛直上方向をｚ軸の正Ｚ
とする固定直交座標Ｏ−ｘｙｚを示す図である。
【００３６】図３は、目標の追尾中心を原点Ｏ、目標の
速度ベクトルをｕ軸の正Ｕ、水平面に平行な面内で速度
ベクトルに垂直で右手方向をｖ軸の正Ｖ、速度ベクトル
に垂直で下向きをｗ軸の正Ｗとする動的直交座標Ｏ−ｕ
ｖｗを示す図である。この図において、Ｐは動的直交座
標Ｏ−ｕｖｗのｕ軸回りの角速度、Ｑは動的直交座標Ｏ
−ｕｖｗのｖ軸回りの角速度、Ｒは動的直交座標Ｏ−ｕ
ｖｗのｗ軸回りの角速度である。
速度ベクトルをｕ軸の正Ｕ、水平面に平行な面内で速度
ベクトルに垂直で右手方向をｖ軸の正Ｖ、速度ベクトル
に垂直で下向きをｗ軸の正Ｗとする動的直交座標Ｏ−ｕ
ｖｗを示す図である。この図において、Ｐは動的直交座
標Ｏ−ｕｖｗのｕ軸回りの角速度、Ｑは動的直交座標Ｏ
−ｕｖｗのｖ軸回りの角速度、Ｒは動的直交座標Ｏ−ｕ
ｖｗのｗ軸回りの角速度である。
【００３７】図４は、第１の角速度算出手段３を説明す
るために必要な目標姿勢角を示す図である。この図にお
いて、Ｈは速度ベクトル上の単位ベクトル、Ｉはベクト
ルＨを水平面上に射影したベクトル、Ｊは水平面上にお
ける目標の姿勢角で固定直交座標Ｏ−ｘｙｚのｙ軸Ｙよ
りベクトルＩまでの角度、Ｋは鉛直面における目標の姿
勢角でベクトルＩよりベクトルＨまでの角度、Ｏは固定
直交座標Ｏ−ｘｙｚの原点、Ｘはｘ軸、Ｙはｙ軸、Ｚは
ｚ軸である。
るために必要な目標姿勢角を示す図である。この図にお
いて、Ｈは速度ベクトル上の単位ベクトル、Ｉはベクト
ルＨを水平面上に射影したベクトル、Ｊは水平面上にお
ける目標の姿勢角で固定直交座標Ｏ−ｘｙｚのｙ軸Ｙよ
りベクトルＩまでの角度、Ｋは鉛直面における目標の姿
勢角でベクトルＩよりベクトルＨまでの角度、Ｏは固定
直交座標Ｏ−ｘｙｚの原点、Ｘはｘ軸、Ｙはｙ軸、Ｚは
ｚ軸である。
【００３８】目標姿勢角Ｊ、Ｋの角度をそれぞれα、β
としたとき、次の式１の関係がある。ただし、Ｖｘ、Ｖ
ｙ、Ｖｚは固定直交座標Ｏ−ｘｙｚにおける目標の速度
ベクトルＶのｘ、ｙ、ｚ軸方向の速度成分である。
としたとき、次の式１の関係がある。ただし、Ｖｘ、Ｖ
ｙ、Ｖｚは固定直交座標Ｏ−ｘｙｚにおける目標の速度
ベクトルＶのｘ、ｙ、ｚ軸方向の速度成分である。
【００３９】
【数１】
【００４０】以下の式２で示す固定直交座標Ｏ−ｘｙｚ
の位置と、以下の式３で示す動的直交座標Ｏ−ｕｖｗの
位置との間には、以下の式４の関係がある。
の位置と、以下の式３で示す動的直交座標Ｏ−ｕｖｗの
位置との間には、以下の式４の関係がある。
【００４１】
【数２】
【００４２】
【数３】
【００４３】
【数４】
【００４４】上記式４において、動的直交座標Ｏ−ｕｖ
ｗから固定直交座標Ｏ−ｘｙｚに変換する座標変換行列
Ｆは次の式５で表される。
ｗから固定直交座標Ｏ−ｘｙｚに変換する座標変換行列
Ｆは次の式５で表される。
【００４５】
【数５】
【００４６】固定直交座標Ｏ−ｘｙｚにおける各軸上の
単位ベクトルをそれぞれ、「−ｅx」、「−ｅy」、「−
ｅz」とし、動的直交座標Ｏ−ｕｖｗにおける各軸上の
単位ベクトルをそれぞれ，「−ｅu」、「−ｅv」、「−
ｅw」とし、角速度ベクトルを「−ω」、その大きさを
ωとする。
単位ベクトルをそれぞれ、「−ｅx」、「−ｅy」、「−
ｅz」とし、動的直交座標Ｏ−ｕｖｗにおける各軸上の
単位ベクトルをそれぞれ，「−ｅu」、「−ｅv」、「−
ｅw」とし、角速度ベクトルを「−ω」、その大きさを
ωとする。
【００４７】さらに、固定直交座標Ｏ−ｘｙｚの目標の
速度ベクトルを「−Ｖ」、目標の加速度ベクトルを「−
Ａ」とし、その大きさをそれぞれ、Ｖ、Ａとし、図３に
示す動的直交座標Ｏ−ｕｖｗにおいて、ｕ軸、ｖ軸、ｗ
軸回りの角速度Ｐ、Ｑ、Ｒをそれぞれ、ｐ、ｑ、ｒとす
る。
速度ベクトルを「−Ｖ」、目標の加速度ベクトルを「−
Ａ」とし、その大きさをそれぞれ、Ｖ、Ａとし、図３に
示す動的直交座標Ｏ−ｕｖｗにおいて、ｕ軸、ｖ軸、ｗ
軸回りの角速度Ｐ、Ｑ、Ｒをそれぞれ、ｐ、ｑ、ｒとす
る。
【００４８】そうすると、ベクトル解析の公式により、
以下の式６、式７、式８、式９、式１０、及び式１１の
関係がある。
以下の式６、式７、式８、式９、式１０、及び式１１の
関係がある。
【００４９】
【数６】
【００５０】
【数７】
【００５１】
【数８】
【００５２】
【数９】
【００５３】
【数１０】
【００５４】
【数１１】
【００５５】前提条件として、目標の速度ベクトルの大
きさを一定とし、次の式１２が成り立つとする。
きさを一定とし、次の式１２が成り立つとする。
【００５６】
【数１２】
【００５７】また、速度ベクトルを回転させる角速度を
一定とみなし、次の式１３が成り立つとする。
一定とみなし、次の式１３が成り立つとする。
【００５８】
【数１３】
【００５９】また、目標の位置の推移に無関係な目標進
行方向ｕ軸回りの角速度Ｐを、次の式１４のように零と
みなす。
行方向ｕ軸回りの角速度Ｐを、次の式１４のように零と
みなす。
【００６０】
【数１４】
【００６１】前提条件の式１２より、上記の式９及び式
１０は、それぞれ、次の式１５及び式１６のようにな
る。
１０は、それぞれ、次の式１５及び式１６のようにな
る。
【００６２】
【数１５】
【００６３】
【数１６】
【００６４】上記の式６の両辺を時間微分して、さら
に、前提条件の式１２及び式８を用いると、式７で表さ
れる目標の加速度ベクトル「−Ａ」は、次の式１７で表
される目標の加速度ベクトル「−Ａ」となる。
に、前提条件の式１２及び式８を用いると、式７で表さ
れる目標の加速度ベクトル「−Ａ」は、次の式１７で表
される目標の加速度ベクトル「−Ａ」となる。
【００６５】
【数１７】
【００６６】目標の加速度ベクトル「−Ａ」を表す上記
の式１７の両辺を時間微分して、前提条件の式１２、式
１３、式１５、及び式１６を使用して、次の式１８を得
る。
の式１７の両辺を時間微分して、前提条件の式１２、式
１３、式１５、及び式１６を使用して、次の式１８を得
る。
【００６７】
【数１８】
【００６８】さらに、上記の式１８に、上記の式６を代
入して、次の式１９を得る。
入して、次の式１９を得る。
【００６９】
【数１９】
【００７０】上記の式６及び式１７より、次の式２０を
得る。
得る。
【００７１】
【数２０】
【００７２】上記の式２０及び前提条件の式１４より、
上記の式１１の目標の角速度ベクトルの大きさωは、次
の式２１で得られる。
上記の式１１の目標の角速度ベクトルの大きさωは、次
の式２１で得られる。
【００７３】
【数２１】
【００７４】図１に示す第１の角速度算出手段３は、上
記の式２１を用いる。
記の式２１を用いる。
【００７５】上記の式１９及び式２１より、次の式２２
を得る。
を得る。
【００７６】
【数２２】
【００７７】上記の式２２は、次の式２３で示すよう
に、加速度ベクトル「−Ａ」の時間微分が、目標の角速
度ベクトルの大きさω、及び速度ベクトル「−Ｖ」で表
されることを示す。
に、加速度ベクトル「−Ａ」の時間微分が、目標の角速
度ベクトルの大きさω、及び速度ベクトル「−Ｖ」で表
されることを示す。
【００７８】
【数２３】
【００７９】次に、図１に示す等角速度運動に基づく予
測手段２を説明するために、等角速度運動に基づく状態
遷移行列の導出を行なっていく。
測手段２を説明するために、等角速度運動に基づく状態
遷移行列の導出を行なっていく。
【００８０】ｋサンプリング目の時刻をｔkとし、サン
プリング間隔Ｔkを、次の式２４で表すとする。
プリング間隔Ｔkを、次の式２４で表すとする。
【００８１】
【数２４】
【００８２】時刻ｔkの時の目標の位置ベクトルを以下
の式２５、速度ベクトルを以下の式２６、加速度ベクト
ルを以下の式２７として、状態変数ベクトルを以下の式
２８で表すとする。
の式２５、速度ベクトルを以下の式２６、加速度ベクト
ルを以下の式２７として、状態変数ベクトルを以下の式
２８で表すとする。
【００８３】
【数２５】
【００８４】
【数２６】
【００８５】
【数２７】
【００８６】
【数２８】
【００８７】時刻ｔk-1の回りで、位置ベクトルを表す
式２５、速度ベクトルを表す式２６、加速度ベクトルを
表す式２７をそれぞれ、解析学の公式により展開を行な
い、５次以上の項を打ち切ると、次の式２９、式３０、
及び式３１となる。
式２５、速度ベクトルを表す式２６、加速度ベクトルを
表す式２７をそれぞれ、解析学の公式により展開を行な
い、５次以上の項を打ち切ると、次の式２９、式３０、
及び式３１となる。
【００８８】
【数２９】
【００８９】
【数３０】
【００９０】
【数３１】
【００９１】サンプリング時刻ｔkの時の角速度の大き
さをωk-1とし、上記の式２９、式３０、及び式３１を
それぞれ、上記の式２５、式２６、式２７、及び式２２
を用いて表すと、以下の式３２、式３３、及び式３４と
なる。
さをωk-1とし、上記の式２９、式３０、及び式３１を
それぞれ、上記の式２５、式２６、式２７、及び式２２
を用いて表すと、以下の式３２、式３３、及び式３４と
なる。
【００９２】
【数３２】
【００９３】
【数３３】
【００９４】
【数３４】
【００９５】ベクトルＩを３行３列の単位行列としたと
き、上記の式３２、式３３、及び式３４を状態変数ベク
トルを表す式２８を用いて、等角速度運動に基づく運動
モデルとして次の式３５のように表す。
き、上記の式３２、式３３、及び式３４を状態変数ベク
トルを表す式２８を用いて、等角速度運動に基づく運動
モデルとして次の式３５のように表す。
【００９６】
【数３５】
【００９７】ここで、上記の式３５において、行列およ
びベクトルの右上の添字（ａ）は等角速度運動モデルに
基づくこと意味しており、「−ｘk (a)」はサンプリング
時刻ｔkにおける等角速度運動に基づく状態変数ベクト
ル、「Φk (a)」はサンプリング時刻ｔk-1からｔkへの等
角速度運動に基づく状態ベクトルの状態遷移行列で、次
の式３６で表す。
びベクトルの右上の添字（ａ）は等角速度運動モデルに
基づくこと意味しており、「−ｘk (a)」はサンプリング
時刻ｔkにおける等角速度運動に基づく状態変数ベクト
ル、「Φk (a)」はサンプリング時刻ｔk-1からｔkへの等
角速度運動に基づく状態ベクトルの状態遷移行列で、次
の式３６で表す。
【００９８】
【数３６】
【００９９】また、「Γ1 (a)（ｋ−１）」は、次の式３
７で表される等角速度運動に基づく駆動雑音の変換行列
である。
７で表される等角速度運動に基づく駆動雑音の変換行列
である。
【０１００】
【数３７】
【０１０１】さらに、「−ｗk (a)」は、サンプリング時
刻ｔkにおける式２９を算出した際の４次の項の打ち切
り誤差に相当する等角速度運動に基づく駆動雑音ベクト
ルである。
刻ｔkにおける式２９を算出した際の４次の項の打ち切
り誤差に相当する等角速度運動に基づく駆動雑音ベクト
ルである。
【０１０２】ここで、Ｅ［］を平均を表す記号として、
「−ｗk (a)」は平均を示す以下の式３８、分散を示す以
下の式３９の多変量正規分布に従うとする。
「−ｗk (a)」は平均を示す以下の式３８、分散を示す以
下の式３９の多変量正規分布に従うとする。
【０１０３】
【数３８】
【０１０４】
【数３９】
【０１０５】図１に示す等角速度運動に基づく予測手段
２で用いる状態遷移行列は、上記の式３６を用いる。
２で用いる状態遷移行列は、上記の式３６を用いる。
【０１０６】次に、等速直線運動に基づく運動モデルに
ついて説明する。等速直線運動に基づく運動モデルとし
て、次の式４０のように表す。
ついて説明する。等速直線運動に基づく運動モデルとし
て、次の式４０のように表す。
【０１０７】
【数４０】
【０１０８】ここで、上記の式４０において、行列およ
びベクトルの右上の添字（ｖ）は等速直線運動モデルに
基づくこと意味しており、「−ｘk (v)」はサンプリング
時刻ｔkにおける等速直線運動に基づく状態変数ベクト
ル、「Φk-1 (v)」はサンプリング時刻ｔk-1からｔkへの
等速直線運動に基づく状態ベクトルの状態遷移行列で次
の式４１で表される。
びベクトルの右上の添字（ｖ）は等速直線運動モデルに
基づくこと意味しており、「−ｘk (v)」はサンプリング
時刻ｔkにおける等速直線運動に基づく状態変数ベクト
ル、「Φk-1 (v)」はサンプリング時刻ｔk-1からｔkへの
等速直線運動に基づく状態ベクトルの状態遷移行列で次
の式４１で表される。
【０１０９】
【数４１】
【０１１０】また、「Γ1 (v)（ｋ−１）」は、次の式４
２で表される等速直線運動に基づく駆動雑音の変換行列
である。
２で表される等速直線運動に基づく駆動雑音の変換行列
である。
【０１１１】
【数４２】
【０１１２】さらに、「−ｗk (v)」は、サンプリング時
刻ｔkにおける３次の項の打ち切り誤差に相当する等速
直線運動に基づく駆動雑音ベクトルである。
刻ｔkにおける３次の項の打ち切り誤差に相当する等速
直線運動に基づく駆動雑音ベクトルである。
【０１１３】ここで、Ｅ［］を平均を表す記号として、
「−ｗk (v)」は平均を示す次の式４３、分散を示す次の
式４４の多変量正規分布に従うとする。
「−ｗk (v)」は平均を示す次の式４３、分散を示す次の
式４４の多変量正規分布に従うとする。
【０１１４】
【数４３】
【０１１５】
【数４４】
【０１１６】図１に示す等速直線運動に基づく予測手段
５で用いる状態遷移行列は、上記の式４１を用いる。
５で用いる状態遷移行列は、上記の式４１を用いる。
【０１１７】また、等角速度運動に基づく観測モデル、
及び等速直線運動に基づく観測モデルを、次の式４５、
及び式４６とする。
及び等速直線運動に基づく観測モデルを、次の式４５、
及び式４６とする。
【０１１８】
【数４５】
【０１１９】
【数４６】
【０１２０】各観測モデルを表す式４５、及び式４６に
おいて、「−ｚk (a)」、及び「−ｚk (v)」は観測値ベク
トル、Ｈは次の式４７で表される観測行列である。
おいて、「−ｚk (a)」、及び「−ｚk (v)」は観測値ベク
トル、Ｈは次の式４７で表される観測行列である。
【０１２１】
【数４７】
【０１２２】また、「−ｖk (a)」、及び「−ｖk (v)」
は、それぞれ、次の式４８、及び式４９で示す多変量正
規分布に従うサンプリング時刻ｔkにおける極座標によ
る観測雑音ベクトルである。
は、それぞれ、次の式４８、及び式４９で示す多変量正
規分布に従うサンプリング時刻ｔkにおける極座標によ
る観測雑音ベクトルである。
【０１２３】
【数４８】
【０１２４】
【数４９】
【０１２５】さらに、「Γ2 (a)（ｋ）」、及び「Γ2 (v)
（ｋ）」は、観測雑音ベクトル「−ｖk (a)」、及び「−
ｖk (v)」の変換行列である。
（ｋ）」は、観測雑音ベクトル「−ｖk (a)」、及び「−
ｖk (v)」の変換行列である。
【０１２６】等角速度運動に基づく運動モデルを示す式
３５における駆動雑音ベクトル「−ｗk (a)」と、等角速
度運動に基づく観測モデルを示す式４５における観測雑
音ベクトル「−ｖk (a)」は、それぞれ次の式５０、及び
式５１のように、互いに無相関とする。
３５における駆動雑音ベクトル「−ｗk (a)」と、等角速
度運動に基づく観測モデルを示す式４５における観測雑
音ベクトル「−ｖk (a)」は、それぞれ次の式５０、及び
式５１のように、互いに無相関とする。
【０１２７】
【数５０】
【０１２８】
【数５１】
【０１２９】ここで、等角速度運動モデル、及び等速直
線運動モデルに基づく状態変数ベクトル「−ｘk (a)」、
及び「−ｘk (v)」の予測値ベクトルをそれぞれ「−ｘk
(a)（−）」、「−ｘk (v)（−）」、平滑値ベクトルを
それぞれ「−ｘk (a)（＋）」、「−ｘk (v)（＋）」、予
測誤差共分散行列をそれぞれ「Ｐk (a)（−）」、「Ｐk (
v)（−）」、平滑誤差共分散行列をそれぞれ「Ｐ
k (a)（＋）」、「Ｐk (v)（＋）」、ゲインをそれぞれ
「Ｋk (a)」、「Ｋk (v)」とする。
線運動モデルに基づく状態変数ベクトル「−ｘk (a)」、
及び「−ｘk (v)」の予測値ベクトルをそれぞれ「−ｘk
(a)（−）」、「−ｘk (v)（−）」、平滑値ベクトルを
それぞれ「−ｘk (a)（＋）」、「−ｘk (v)（＋）」、予
測誤差共分散行列をそれぞれ「Ｐk (a)（−）」、「Ｐk (
v)（−）」、平滑誤差共分散行列をそれぞれ「Ｐ
k (a)（＋）」、「Ｐk (v)（＋）」、ゲインをそれぞれ
「Ｋk (a)」、「Ｋk (v)」とする。
【０１３０】次の式５２のように、等角速度運動モデル
に基づく予測値ベクトルの位置成分と観測値ベクトルの
差のユークリッドノルム、及び以下の式５３のように、
等速直線運動モデルに基づく予測値ベクトルの位置成分
と観測値ベクトルの差のユークリッドノルムを位置の残
差とする。
に基づく予測値ベクトルの位置成分と観測値ベクトルの
差のユークリッドノルム、及び以下の式５３のように、
等速直線運動モデルに基づく予測値ベクトルの位置成分
と観測値ベクトルの差のユークリッドノルムを位置の残
差とする。
【０１３１】
【数５２】
【０１３２】
【数５３】
【０１３３】このとき、以下の式５４、式５５のよう
に、等角速度運動モデルに基づく予測処理、および等速
直線運動に基づく予測処理の結果、算出された等角速度
運動に基づく予測値ベクトル「−ｘk (a)（−）」、およ
び等速直線運動に基づく予測値ベクトル「−ｘ
k (v)（−）」の内、予測値ベクトルの位置成分と観測値
ベクトルの残差を算出し、その残差が最小になる予測値
ベクトル、予測誤差共分散行列を選択し、それをそれぞ
れ「−ｘk（−）」、「Ｐk（−）」とする。
に、等角速度運動モデルに基づく予測処理、および等速
直線運動に基づく予測処理の結果、算出された等角速度
運動に基づく予測値ベクトル「−ｘk (a)（−）」、およ
び等速直線運動に基づく予測値ベクトル「−ｘ
k (v)（−）」の内、予測値ベクトルの位置成分と観測値
ベクトルの残差を算出し、その残差が最小になる予測値
ベクトル、予測誤差共分散行列を選択し、それをそれぞ
れ「−ｘk（−）」、「Ｐk（−）」とする。
【０１３４】
【数５４】
【０１３５】
【数５５】
【０１３６】以上より、カルマンフィルタの理論に基づ
き、以下の式５６、式５７、式５８、及び式５９を用い
て予測処理を行う。
き、以下の式５６、式５７、式５８、及び式５９を用い
て予測処理を行う。
【０１３７】
【数５６】
【０１３８】
【数５７】
【０１３９】
【数５８】
【０１４０】
【数５９】
【０１４１】上記の予測処理で算出された等角速度運動
に基づく予測値ベクトル、予測誤差共分散行列、および
等速直線運動に基づく予測値ベクトル、予測誤差共分散
行列から、上記の式５４、式５５を用いて、予測値ベク
トルの位置成分と観測値ベクトルの残差が最小になる予
測値ベクトル「−ｘk（−）」、予測誤差共分散行列
「Ｐk（−）」を算出する。
に基づく予測値ベクトル、予測誤差共分散行列、および
等速直線運動に基づく予測値ベクトル、予測誤差共分散
行列から、上記の式５４、式５５を用いて、予測値ベク
トルの位置成分と観測値ベクトルの残差が最小になる予
測値ベクトル「−ｘk（−）」、予測誤差共分散行列
「Ｐk（−）」を算出する。
【０１４２】予測値ベクトルの位置成分と観測値ベクト
ルの残差が最小になる予測値ベクトル「−ｘ
k（−）」、および予測誤差共分散行列「Ｐk（−）」か
ら、以下の式６０、式６１、式６２、式６３、式６４、
及び式６５を用いて平滑処理を行なう。
ルの残差が最小になる予測値ベクトル「−ｘ
k（−）」、および予測誤差共分散行列「Ｐk（−）」か
ら、以下の式６０、式６１、式６２、式６３、式６４、
及び式６５を用いて平滑処理を行なう。
【０１４３】
【数６０】
【０１４４】
【数６１】
【０１４５】
【数６２】
【０１４６】
【数６３】
【０１４７】
【数６４】
【０１４８】
【数６５】
【０１４９】次に、この実施の形態１に係る追尾装置の
動作について図面を参照しながら説明する。図５は、こ
の発明の実施の形態１に係る追尾装置の動作を示すフロ
ーチャートである。
動作について図面を参照しながら説明する。図５は、こ
の発明の実施の形態１に係る追尾装置の動作を示すフロ
ーチャートである。
【０１５０】最初に、ステップＳＴ１の観測値入力にお
いて、極座標系で与えられる目標位置情報を固定直交座
標に直した観測値ベクトルを求める。
いて、極座標系で与えられる目標位置情報を固定直交座
標に直した観測値ベクトルを求める。
【０１５１】次に、ステップＳＴ２の等角速度運動に基
づく平滑においては、ステップＳＴ１の観測値入力で算
出した観測値ベクトルと、前もって計算してある観測誤
差共分散行列を用いて、上記の式６０に基づき、等角速
度運動に基づくゲイン行列を算出する。
づく平滑においては、ステップＳＴ１の観測値入力で算
出した観測値ベクトルと、前もって計算してある観測誤
差共分散行列を用いて、上記の式６０に基づき、等角速
度運動に基づくゲイン行列を算出する。
【０１５２】それから、この等角速度運動に基づくゲイ
ン行列と前もって求めてある観測値ベクトル、また、等
角速度運動に基づく予測値ベクトルおよび等速直線運動
に基づく予測値ベクトルの内、予測値ベクトルの位置成
分と観測値ベクトルの残差が最小になるように選択され
た予測値ベクトルおよび予測誤差共分散行列を用いて上
記の式６２に基づき、平滑値ベクトルを算出する。
ン行列と前もって求めてある観測値ベクトル、また、等
角速度運動に基づく予測値ベクトルおよび等速直線運動
に基づく予測値ベクトルの内、予測値ベクトルの位置成
分と観測値ベクトルの残差が最小になるように選択され
た予測値ベクトルおよび予測誤差共分散行列を用いて上
記の式６２に基づき、平滑値ベクトルを算出する。
【０１５３】さらに、上記の式６０から算出したゲイン
行列および選択された予測誤差共分散行列から上記の式
６４を用いて平滑誤差共分散行列を算出する。
行列および選択された予測誤差共分散行列から上記の式
６４を用いて平滑誤差共分散行列を算出する。
【０１５４】次に、ステップＳＴ３の角速度算出では、
前もって算出してある予測値ベクトルまたは平滑値ベク
トルの速度・加速度成分より上記の式２１を用いて、角
速度を算出する。
前もって算出してある予測値ベクトルまたは平滑値ベク
トルの速度・加速度成分より上記の式２１を用いて、角
速度を算出する。
【０１５５】次に、ステップＳＴ４の等角速度運動に基
づく予測処理では、ステップＳＴ３の角速度算出で算出
してある角速度を用いて、上記の式３６を用いて等角速
度運動に基づく状態遷移行列を算出する。
づく予測処理では、ステップＳＴ３の角速度算出で算出
してある角速度を用いて、上記の式３６を用いて等角速
度運動に基づく状態遷移行列を算出する。
【０１５６】それから、ステップＳＴ２の等角速度運動
に基づく平滑において算出した平滑値と、上記の式３６
を用いて算出した等角速度運動に基づく状態遷移行列か
ら上記の式５６に基づき等角速度運動に基づく予測値ベ
クトルを算出する。
に基づく平滑において算出した平滑値と、上記の式３６
を用いて算出した等角速度運動に基づく状態遷移行列か
ら上記の式５６に基づき等角速度運動に基づく予測値ベ
クトルを算出する。
【０１５７】さらに、ステップＳＴ２の等角速度運動に
基づく平滑において算出した平滑誤差共分散行列と、上
記の式３６を用いて算出した等角速度運動に基づく状態
遷移行列から上記の式５８に基づき予測誤差共分散行列
を算出する。
基づく平滑において算出した平滑誤差共分散行列と、上
記の式３６を用いて算出した等角速度運動に基づく状態
遷移行列から上記の式５８に基づき予測誤差共分散行列
を算出する。
【０１５８】次に、ステップＳＴ５の等速直線運動に基
づく平滑においては、ステップＳＴ１の観測値入力で算
出した観測値ベクトルと前もって計算してある観測誤差
共分散行列を用いて、上記の式６１に基づき、等速直線
運動に基づくゲイン行列を算出する。
づく平滑においては、ステップＳＴ１の観測値入力で算
出した観測値ベクトルと前もって計算してある観測誤差
共分散行列を用いて、上記の式６１に基づき、等速直線
運動に基づくゲイン行列を算出する。
【０１５９】それから、この等速直線運動に基づくゲイ
ン行列と前もって求めてある観測値ベクトル、また、等
角速度運動に基づく予測値ベクトルおよび等速直線運動
に基づく予測値ベクトルの内、予測値ベクトルの位置成
分と観測値ベクトルの残差が最小になるような選択され
た予測値ベクトルを用いて上記の式６３に基づき、平滑
値ベクトルを算出する。
ン行列と前もって求めてある観測値ベクトル、また、等
角速度運動に基づく予測値ベクトルおよび等速直線運動
に基づく予測値ベクトルの内、予測値ベクトルの位置成
分と観測値ベクトルの残差が最小になるような選択され
た予測値ベクトルを用いて上記の式６３に基づき、平滑
値ベクトルを算出する。
【０１６０】さらに、上記の式６１から算出したゲイン
行列および選択された予測誤差共分散行列から上記の式
６５を用いて平滑誤差共分散行列を算出する。
行列および選択された予測誤差共分散行列から上記の式
６５を用いて平滑誤差共分散行列を算出する。
【０１６１】次に、ステップＳＴ６の等速直線運動に基
づく予測処理では、ステップＳＴ５の等速直線運動に基
づく平滑において算出した平滑値と、上記の式４１の等
速直線運動に基づく状態遷移行列から、上記の式５７に
基づき等速直線運動に基づく予測値ベクトルを算出す
る。
づく予測処理では、ステップＳＴ５の等速直線運動に基
づく平滑において算出した平滑値と、上記の式４１の等
速直線運動に基づく状態遷移行列から、上記の式５７に
基づき等速直線運動に基づく予測値ベクトルを算出す
る。
【０１６２】さらに、ステップＳＴ５の等速直線運動に
基づく平滑において算出した平滑誤差共分散行列と、上
記の式４１の等速直線運動に基づく状態遷移行列から、
上記の式５９に基づき予測誤差共分散行列を算出する。
基づく平滑において算出した平滑誤差共分散行列と、上
記の式４１の等速直線運動に基づく状態遷移行列から、
上記の式５９に基づき予測誤差共分散行列を算出する。
【０１６３】次に、ステップＳＴ７の予測値選択処理で
は、ステップＳＴ４で算出した等角速度運動に基づく予
測値ベクトル、予測誤差共分散行列、およびステップＳ
Ｔ６で算出した等速直線運動に基づく予測値ベクトル、
予測誤差共分散行列から、上記の式５４を用いて予測値
ベクトルの位置成分と観測値ベクトルの残差が最小にな
る予測値ベクトルを選択する。
は、ステップＳＴ４で算出した等角速度運動に基づく予
測値ベクトル、予測誤差共分散行列、およびステップＳ
Ｔ６で算出した等速直線運動に基づく予測値ベクトル、
予測誤差共分散行列から、上記の式５４を用いて予測値
ベクトルの位置成分と観測値ベクトルの残差が最小にな
る予測値ベクトルを選択する。
【０１６４】そして、ステップＳＴ８では、平滑処理を
行なうために、処理を続ける場合には、遅延要素により
ステップＳＴ１の観測値入力の処理に戻す。
行なうために、処理を続ける場合には、遅延要素により
ステップＳＴ１の観測値入力の処理に戻す。
【０１６５】従って、この実施の形態１によれば、固定
直交座標において、等角速度運動に基づく予測・平滑処
理、および等速直線運動に基づく予測・平滑処理を併用
することにより、直線および旋回に対する追従性、追尾
精度が向上する。
直交座標において、等角速度運動に基づく予測・平滑処
理、および等速直線運動に基づく予測・平滑処理を併用
することにより、直線および旋回に対する追従性、追尾
精度が向上する。
【０１６６】すなわち、この実施の形態１に係る追尾装
置は、目標の位置推移に無関係な速度ベクトル方向の角
速度を０、目標の速度ベクトルを回転させる角速度を一
定、目標進行方向の速度を一定とするという前提条件を
用いて、固定直交座標において角速度ベクトルを求め、
目標位置ベクトルの３次微分を角速度ベクトルの大きさ
と目標位置ベクトルの１次微分から算出して、等角速度
運動に基づく追尾フィルタを構成し、さらに等速直線運
動に基づく追尾フィルタを併用することによって、旋回
目標および直進目標に対しても精度良く追尾できる。ま
た、従来の追尾装置と比べて、固定直交座標−動的直交
座標間で座標変換が不要であるので、アルゴリズムが簡
略化されている。
置は、目標の位置推移に無関係な速度ベクトル方向の角
速度を０、目標の速度ベクトルを回転させる角速度を一
定、目標進行方向の速度を一定とするという前提条件を
用いて、固定直交座標において角速度ベクトルを求め、
目標位置ベクトルの３次微分を角速度ベクトルの大きさ
と目標位置ベクトルの１次微分から算出して、等角速度
運動に基づく追尾フィルタを構成し、さらに等速直線運
動に基づく追尾フィルタを併用することによって、旋回
目標および直進目標に対しても精度良く追尾できる。ま
た、従来の追尾装置と比べて、固定直交座標−動的直交
座標間で座標変換が不要であるので、アルゴリズムが簡
略化されている。
【０１６７】実施の形態２．この発明の実施の形態２に
係る追尾装置について図面を参照しながら説明する。図
６は、この発明の実施の形態２に係る追尾装置の構成を
示すブロック図である。なお、各図中、同一符号は同一
又は相当部分を示す。
係る追尾装置について図面を参照しながら説明する。図
６は、この発明の実施の形態２に係る追尾装置の構成を
示すブロック図である。なお、各図中、同一符号は同一
又は相当部分を示す。
【０１６８】図６において、１は観測手段、２は等角速
度運動に基づく予測手段、３は第１の角速度算出手段、
４は第１の遅延要素、５は等速直線運動に基づく予測手
段、７は第２の遅延要素、８は等角速度運動に基づく平
滑手段、９は等速直線運動に基づく平滑手段、１０は残
差による予測値結合手段である。
度運動に基づく予測手段、３は第１の角速度算出手段、
４は第１の遅延要素、５は等速直線運動に基づく予測手
段、７は第２の遅延要素、８は等角速度運動に基づく平
滑手段、９は等速直線運動に基づく平滑手段、１０は残
差による予測値結合手段である。
【０１６９】残差による予測値統合手段１０は、予測値
ベクトルの位置成分と観測値ベクトルの残差で、等角速
度運動に基づく予測値ベクトル、予測誤差共分散行列と
等速直線運動に基づく予測値ベクトル、予測誤差共分散
行列を重み付けしたものを予測値ベクトルとする。
ベクトルの位置成分と観測値ベクトルの残差で、等角速
度運動に基づく予測値ベクトル、予測誤差共分散行列と
等速直線運動に基づく予測値ベクトル、予測誤差共分散
行列を重み付けしたものを予測値ベクトルとする。
【０１７０】次に、この実施の形態２に係る追尾装置の
動作について図面を参照しながら説明する。図７は、こ
の発明の実施の形態２に係る追尾装置の動作を示すフロ
ーチャートである。なお、上記実施の形態１と同一動作
部分には同一符号を付して説明を省略する。
動作について図面を参照しながら説明する。図７は、こ
の発明の実施の形態２に係る追尾装置の動作を示すフロ
ーチャートである。なお、上記実施の形態１と同一動作
部分には同一符号を付して説明を省略する。
【０１７１】ステップＳＴ７ａの残差による予測値統合
処理において、等角速度運動に基づく予測値ベクトルと
等速直線運動に基づく予測値ベクトルから、上記の式５
２、式５３を用いて、予測値ベクトルの位置成分と観測
値ベクトルの残差を算出し、次の式６６、式６７を用い
て、残差で重みづけした予測値ベクトルおよび予測誤差
共分散行列を算出する。
処理において、等角速度運動に基づく予測値ベクトルと
等速直線運動に基づく予測値ベクトルから、上記の式５
２、式５３を用いて、予測値ベクトルの位置成分と観測
値ベクトルの残差を算出し、次の式６６、式６７を用い
て、残差で重みづけした予測値ベクトルおよび予測誤差
共分散行列を算出する。
【０１７２】
【数６６】
【０１７３】
【数６７】
【０１７４】従って、この実施の形態２によれば、固定
直交座標において、等角速度運動に基づく予測・平滑処
理および等速直線運動に基づく予測・平滑処理を併用す
ることにより、直線および旋回に対する追従性、追尾精
度が向上する。
直交座標において、等角速度運動に基づく予測・平滑処
理および等速直線運動に基づく予測・平滑処理を併用す
ることにより、直線および旋回に対する追従性、追尾精
度が向上する。
【０１７５】実施の形態３．この発明の実施の形態３に
係る追尾装置について図面を参照しながら説明する。図
８は、この発明の実施の形態３に係る追尾装置の構成を
示すブロック図である。なお、各図中、同一符号は同一
又は相当部分を示す。
係る追尾装置について図面を参照しながら説明する。図
８は、この発明の実施の形態３に係る追尾装置の構成を
示すブロック図である。なお、各図中、同一符号は同一
又は相当部分を示す。
【０１７６】図８において、１は観測手段、２は等角速
度運動に基づく予測手段、３は第１の角速度算出手段、
４は第１の遅延要素、５は等速直線運動に基づく予測手
段、７は第２の遅延要素、８は等角速度運動に基づく平
滑手段、９は等速直線運動に基づく平滑手段、１１は角
速度による予測値選択手段である。
度運動に基づく予測手段、３は第１の角速度算出手段、
４は第１の遅延要素、５は等速直線運動に基づく予測手
段、７は第２の遅延要素、８は等角速度運動に基づく平
滑手段、９は等速直線運動に基づく平滑手段、１１は角
速度による予測値選択手段である。
【０１７７】角速度による予測値選択手段１１は、等角
速度運動に基づく予測値ベクトル、予測誤差共分散行列
と等速直線運動に基づく予測値ベクトル、予測誤差共分
散行列を角速度の大きさにより選択する。
速度運動に基づく予測値ベクトル、予測誤差共分散行列
と等速直線運動に基づく予測値ベクトル、予測誤差共分
散行列を角速度の大きさにより選択する。
【０１７８】次に、この実施の形態３に係る追尾装置の
動作について図面を参照しながら説明する。図９は、こ
の発明の実施の形態３に係る追尾装置の動作を示すフロ
ーチャートである。なお、上記実施の形態１と同一動作
部分には同一符号を付して説明を省略する。
動作について図面を参照しながら説明する。図９は、こ
の発明の実施の形態３に係る追尾装置の動作を示すフロ
ーチャートである。なお、上記実施の形態１と同一動作
部分には同一符号を付して説明を省略する。
【０１７９】ステップＳＴ７ｂの角速度による予測値選
択処理では、ステップＳＴ３で算出した角速度が、事前
に決めておいたある一定の閾値を越える場合は、等角速
度運動に基づく予測値ベクトル、予測誤差共分散行列を
選択し、越えない場合には、等速直線運動に基づく予測
値ベクトル、予測誤差共分散行列を選択する。
択処理では、ステップＳＴ３で算出した角速度が、事前
に決めておいたある一定の閾値を越える場合は、等角速
度運動に基づく予測値ベクトル、予測誤差共分散行列を
選択し、越えない場合には、等速直線運動に基づく予測
値ベクトル、予測誤差共分散行列を選択する。
【０１８０】従って、この実施の形態３によれば、固定
直交座標において、等角速度運動に基づく予測・平滑処
理および等速直線運動に基づく予測・平滑処理を併用す
ることにより、直線および旋回に対する追従性、追尾精
度が向上する。
直交座標において、等角速度運動に基づく予測・平滑処
理および等速直線運動に基づく予測・平滑処理を併用す
ることにより、直線および旋回に対する追従性、追尾精
度が向上する。
【０１８１】実施の形態４．この発明の実施の形態４に
係る追尾装置について図面を参照しながら説明する。図
１０は、この発明の実施の形態４に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
係る追尾装置について図面を参照しながら説明する。図
１０は、この発明の実施の形態４に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
【０１８２】図１０において、１は観測手段、２は等角
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１２は目標存在期待領域選
択手段、１３は第３の遅延要素、１４は目標位置情報評
価手段、１５は信頼度算出手段である。
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１２は目標存在期待領域選
択手段、１３は第３の遅延要素、１４は目標位置情報評
価手段、１５は信頼度算出手段である。
【０１８３】目標存在期待領域選択手段１２は、等角速
度運動に基づく予測手段２で算出される予測値ベクトル
「−ｘk (a)（−）」から以下の式６８を用いて変換した
予測観測値「−ｚk (a)（−）」、等速直線運動に基づく
予測手段５で算出される予測値ベクトル「−ｘ
k (v)（−）」から以下の式６９を用いて変換した予測観
測値「−ｚk (v)（−）」、等角速度運動に基づく予測手
段２で算出される予測誤差共分散行列「Ｐk (a)（−）」
から以下の式７０を用いて変換した予測観測値の誤差共
分散行列「Ｓk (a)（−）」、等速直線運動に基づく予測
手段５で算出される予測誤差共分散行列「Ｐ
k (v)（−）」から以下の式７１を用いて変換した予測観
測値の誤差共分散行列「Ｓk (v)（−）」を用いて、以下
の式７２、式７３でそれぞれ表される等角速度運動に基
づく目標存在期待領域、等速直線運動に基づく目標存在
期待領域から、以下の式７０より算出される「Ｓ
k (a)（−）」、以下の式７１より算出される「Ｓ
k (v)（−）」内、それらのスペクトルノルム｜Ｓ
k (a)（−）｜、｜Ｓk (v)（−）｜が最大となる目標存在
期待領域を選択する。
度運動に基づく予測手段２で算出される予測値ベクトル
「−ｘk (a)（−）」から以下の式６８を用いて変換した
予測観測値「−ｚk (a)（−）」、等速直線運動に基づく
予測手段５で算出される予測値ベクトル「−ｘ
k (v)（−）」から以下の式６９を用いて変換した予測観
測値「−ｚk (v)（−）」、等角速度運動に基づく予測手
段２で算出される予測誤差共分散行列「Ｐk (a)（−）」
から以下の式７０を用いて変換した予測観測値の誤差共
分散行列「Ｓk (a)（−）」、等速直線運動に基づく予測
手段５で算出される予測誤差共分散行列「Ｐ
k (v)（−）」から以下の式７１を用いて変換した予測観
測値の誤差共分散行列「Ｓk (v)（−）」を用いて、以下
の式７２、式７３でそれぞれ表される等角速度運動に基
づく目標存在期待領域、等速直線運動に基づく目標存在
期待領域から、以下の式７０より算出される「Ｓ
k (a)（−）」、以下の式７１より算出される「Ｓ
k (v)（−）」内、それらのスペクトルノルム｜Ｓ
k (a)（−）｜、｜Ｓk (v)（−）｜が最大となる目標存在
期待領域を選択する。
【０１８４】
【数６８】
【０１８５】
【数６９】
【０１８６】
【数７０】
【０１８７】
【数７１】
【０１８８】
【数７２】
【０１８９】
【数７３】
【０１９０】ここで、上記の式７２、式７３におけるｄ
は、カイ２乗分布表から算出する。
は、カイ２乗分布表から算出する。
【０１９１】目標位置情報評価手段１４は、第３の遅延
要素１３を通して入力された、目標存在期待領域選択手
段１２において選択された予測観測値の誤差共分散行列
が最大になる目標存在期待領域を用いて、観測手段１か
ら求められる観測値ベクトル「−ｚk」がその目標存在
期待領域内に存在するか否かを判定する。
要素１３を通して入力された、目標存在期待領域選択手
段１２において選択された予測観測値の誤差共分散行列
が最大になる目標存在期待領域を用いて、観測手段１か
ら求められる観測値ベクトル「−ｚk」がその目標存在
期待領域内に存在するか否かを判定する。
【０１９２】信頼度算出手段１５は、目標存在期待領域
内に存在する観測値ベクトルの内、目標存在期待領域内
の中心からの距離に応じて算出される確率密度により信
頼度を算出する。
内に存在する観測値ベクトルの内、目標存在期待領域内
の中心からの距離に応じて算出される確率密度により信
頼度を算出する。
【０１９３】図１１は、目標存在期待領域およびその信
頼度の概念を表す図である。
頼度の概念を表す図である。
【０１９４】次に、この実施の形態４に係る追尾装置の
動作について図面を参照しながら説明する。図１２は、
この発明の実施の形態４に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
動作について図面を参照しながら説明する。図１２は、
この発明の実施の形態４に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
【０１９５】ステップＳＴ９の等角速度運動に基づく目
標存在期待領域算出による予測値選択処理では、ステッ
プＳＴ４で算出した予測値ベクトル、予測誤差共分散行
列により予測観測値の誤差共分散行列を算出し、それか
ら目標存在期待領域を算出する。
標存在期待領域算出による予測値選択処理では、ステッ
プＳＴ４で算出した予測値ベクトル、予測誤差共分散行
列により予測観測値の誤差共分散行列を算出し、それか
ら目標存在期待領域を算出する。
【０１９６】ステップＳＴ１０の等速直線運動に基づく
目標存在期待領域算出による予測値選択処理では、ステ
ップＳＴ６で算出した予測値ベクトル、予測誤差共分散
行列により予測観測値の誤差共分散行列を算出し、それ
から目標存在期待領域を算出する。
目標存在期待領域算出による予測値選択処理では、ステ
ップＳＴ６で算出した予測値ベクトル、予測誤差共分散
行列により予測観測値の誤差共分散行列を算出し、それ
から目標存在期待領域を算出する。
【０１９７】ステップＳＴ１１の目標存在期待領域選択
処理では、等角速度運動に基づく目標存在期待領域およ
び等速直線運動に基づく目標存在期待領域の内、予測誤
差共分散行列のスペクトルノルムが最小になる目標存在
期待領域を選択する。
処理では、等角速度運動に基づく目標存在期待領域およ
び等速直線運動に基づく目標存在期待領域の内、予測誤
差共分散行列のスペクトルノルムが最小になる目標存在
期待領域を選択する。
【０１９８】ステップＳＴ１２の観測値判定および信頼
度算出では、ステップＳＴ１１で得られた目標存在期待
領域内に存在するステップＳＴ１で得られる観測値ベク
トルを元に、目標存在期待領域内の中心からの距離に応
じて算出される確率密度により信頼度を算出する。
度算出では、ステップＳＴ１１で得られた目標存在期待
領域内に存在するステップＳＴ１で得られる観測値ベク
トルを元に、目標存在期待領域内の中心からの距離に応
じて算出される確率密度により信頼度を算出する。
【０１９９】ステップＳＴ２ａの信頼度による等角速度
運動に基づく平滑では、ステップＳＴ４の等角速度運動
に基づく予測より算出された予測値ベクトル、予測誤差
共分散行列、状態遷移行列およびステップＳＴ１の観測
値入力より得られる観測値ベクトルよりステップＳＴ１
２で算出された信頼度により観測値ベクトル、予測値ベ
クトル、予測誤差共分散行列の重み付けを行ない平滑値
ベクトル、平滑誤差共分散行列を算出する。
運動に基づく平滑では、ステップＳＴ４の等角速度運動
に基づく予測より算出された予測値ベクトル、予測誤差
共分散行列、状態遷移行列およびステップＳＴ１の観測
値入力より得られる観測値ベクトルよりステップＳＴ１
２で算出された信頼度により観測値ベクトル、予測値ベ
クトル、予測誤差共分散行列の重み付けを行ない平滑値
ベクトル、平滑誤差共分散行列を算出する。
【０２００】ステップＳＴ５ａの信頼度による等速直線
運動に基づく平滑では、ステップＳＴ６の等速直線運動
に基づく予測より算出された予測値ベクトル、予測誤差
共分散行列、状態遷移行列およびステップＳＴ１の観測
値入力より得られる観測値ベクトルよりステップＳＴ１
２で算出された信頼度により観測値ベクトル、予測値ベ
クトル、予測誤差共分散行列の重み付けを行ない平滑値
ベクトル、平滑誤差共分散行列を算出する。
運動に基づく平滑では、ステップＳＴ６の等速直線運動
に基づく予測より算出された予測値ベクトル、予測誤差
共分散行列、状態遷移行列およびステップＳＴ１の観測
値入力より得られる観測値ベクトルよりステップＳＴ１
２で算出された信頼度により観測値ベクトル、予測値ベ
クトル、予測誤差共分散行列の重み付けを行ない平滑値
ベクトル、平滑誤差共分散行列を算出する。
【０２０１】従って、この実施の形態４によれば、目標
存在期待領域を算出して、観測値ベクトルを目標存在期
待領域内に存在するか否かを判定し、信頼度を算出する
ことによって、不要信号の無い自由空間のみならず、不
要信号環境下においても追従性および追尾精度が確保で
きる。
存在期待領域を算出して、観測値ベクトルを目標存在期
待領域内に存在するか否かを判定し、信頼度を算出する
ことによって、不要信号の無い自由空間のみならず、不
要信号環境下においても追従性および追尾精度が確保で
きる。
【０２０２】実施の形態５．この発明の実施の形態５に
係る追尾装置について図面を参照しながら説明する。図
１３は、この発明の実施の形態５に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
係る追尾装置について図面を参照しながら説明する。図
１３は、この発明の実施の形態５に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
【０２０３】図１３において、１は観測手段、２は等角
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１６は残差によるサンプリ
ング制御手段である。
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１６は残差によるサンプリ
ング制御手段である。
【０２０４】残差によるサンプリング制御手段１６は、
予測値ベクトルの位置成分と観測値ベクトルの残差によ
り、目標の運動状態を判定し、サンプリングレートを変
える。
予測値ベクトルの位置成分と観測値ベクトルの残差によ
り、目標の運動状態を判定し、サンプリングレートを変
える。
【０２０５】次に、この実施の形態５に係る追尾装置の
動作について図面を参照しながら説明する。図１４は、
この発明の実施の形態５に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
動作について図面を参照しながら説明する。図１４は、
この発明の実施の形態５に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
【０２０６】ステップＳＴ１３の残差によるサンプリン
グレート制御において、予測誤差により目標の運動状態
を判定し、目標の運動状態がわかっている場合は、ステ
ップＳＴ２からステップＳＴ７までのサンプリングレー
トを荒くし、目標の運動状態がわからない場合には、ス
テップＳＴ２からステップＳＴ７までのサンプリングレ
ートを細かくして、サンプリングレートを制御する。
グレート制御において、予測誤差により目標の運動状態
を判定し、目標の運動状態がわかっている場合は、ステ
ップＳＴ２からステップＳＴ７までのサンプリングレー
トを荒くし、目標の運動状態がわからない場合には、ス
テップＳＴ２からステップＳＴ７までのサンプリングレ
ートを細かくして、サンプリングレートを制御する。
【０２０７】従って、この実施の形態５によれば、予測
誤差の大きさから目標の運動状態を判定し、サンプリン
グレートを変化させることによって、目標の運動状態が
わかっているときは、サンプリングを荒くするので、計
算量の軽減になり、目標の運動状態がわからないときに
は、サンプリングを細かくすることで、目標の運動状態
が明らかになり、追尾精度も向上する。
誤差の大きさから目標の運動状態を判定し、サンプリン
グレートを変化させることによって、目標の運動状態が
わかっているときは、サンプリングを荒くするので、計
算量の軽減になり、目標の運動状態がわからないときに
は、サンプリングを細かくすることで、目標の運動状態
が明らかになり、追尾精度も向上する。
【０２０８】実施の形態６．この発明の実施の形態６に
係る追尾装置について図面を参照しながら説明する。図
１５は、この発明の実施の形態６に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
係る追尾装置について図面を参照しながら説明する。図
１５は、この発明の実施の形態６に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
【０２０９】図１５において、１は観測手段、２は等角
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１７は角速度によるサンプ
リング制御手段である。
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１７は角速度によるサンプ
リング制御手段である。
【０２１０】角速度によるサンプリング制御手段１７
は、角速度によって、目標の運動状態を判定し、サンプ
リングレートを変える。
は、角速度によって、目標の運動状態を判定し、サンプ
リングレートを変える。
【０２１１】次に、この実施の形態６に係る追尾装置の
動作について図面を参照しながら説明する。図１６は、
この発明の実施の形態６に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
動作について図面を参照しながら説明する。図１６は、
この発明の実施の形態６に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
【０２１２】ステップＳＴ１３ａの角速度によるサンプ
リングレート制御において、角速度により目標の運動状
態を判定し、目標の運動状態がわかっている場合は、ス
テップＳＴ２からステップＳＴ７までのサンプリングレ
ートを荒くし、目標の運動状態がわからない場合には、
ステップＳＴ２からステップＳＴ７までのサンプリング
レートを細かくして、サンプリングレートを制御する。
リングレート制御において、角速度により目標の運動状
態を判定し、目標の運動状態がわかっている場合は、ス
テップＳＴ２からステップＳＴ７までのサンプリングレ
ートを荒くし、目標の運動状態がわからない場合には、
ステップＳＴ２からステップＳＴ７までのサンプリング
レートを細かくして、サンプリングレートを制御する。
【０２１３】従って、この実施の形態６によれば、角速
度の大きさから目標の運動状態を判定し、サンプリング
レートを変化させることによって、目標の運動状態がわ
かっているときは、サンプリングを荒くするので、計算
量の軽減になり、目標の運動状態がわからないときに
は、サンプリングを細かくすることで、目標の運動状態
が明らかになり、追尾精度も向上する。
度の大きさから目標の運動状態を判定し、サンプリング
レートを変化させることによって、目標の運動状態がわ
かっているときは、サンプリングを荒くするので、計算
量の軽減になり、目標の運動状態がわからないときに
は、サンプリングを細かくすることで、目標の運動状態
が明らかになり、追尾精度も向上する。
【０２１４】実施の形態７．この発明の実施の形態７に
係る追尾装置について図面を参照しながら説明する。図
１７は、この発明の実施の形態７に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
係る追尾装置について図面を参照しながら説明する。図
１７は、この発明の実施の形態７に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
【０２１５】図１７において、１は観測手段、２は等角
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１８は表示手段である。
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、６は残差による予測値選択手段、７は第２の遅
延要素、８は等角速度運動に基づく平滑手段、９は等速
直線運動に基づく平滑手段、１８は表示手段である。
【０２１６】表示手段１８は、上記等角速度運動に基づ
く予測処理２より算出される予測値および上記等速直線
運動に基づく予測処理５より算出される予測値の内、残
差による予測値選択手段６によって得られた予測値１つ
のみを表示する。
く予測処理２より算出される予測値および上記等速直線
運動に基づく予測処理５より算出される予測値の内、残
差による予測値選択手段６によって得られた予測値１つ
のみを表示する。
【０２１７】次に、この実施の形態７に係る追尾装置の
動作について図面を参照しながら説明する。図１８は、
この発明の実施の形態７に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
動作について図面を参照しながら説明する。図１８は、
この発明の実施の形態７に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態１と同一動
作部分には同一符号を付して説明を省略する。
【０２１８】ステップＳＴ１４の目標位置表示処理にお
いて、ステップＳＴ７の予測値選択処理によって得られ
た予測値１つのみを表示する。
いて、ステップＳＴ７の予測値選択処理によって得られ
た予測値１つのみを表示する。
【０２１９】従って、この実施の形態７によれば、等速
直線運動に基づく予測値および等角速度運動に基づく予
測値の全てを表示するのでなく、その中の１つの予測値
を選択し、その予測値のみを表示させることで、レーダ
ーを制御するオペレーターにとって表示が見やすくな
る。
直線運動に基づく予測値および等角速度運動に基づく予
測値の全てを表示するのでなく、その中の１つの予測値
を選択し、その予測値のみを表示させることで、レーダ
ーを制御するオペレーターにとって表示が見やすくな
る。
【０２２０】実施の形態８．この発明の実施の形態８に
係る追尾装置について図面を参照しながら説明する。図
１９は、この発明の実施の形態８に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
係る追尾装置について図面を参照しながら説明する。図
１９は、この発明の実施の形態８に係る追尾装置の構成
を示すブロック図である。なお、各図中、同一符号は同
一又は相当部分を示す。
【０２２１】図１９において、１は観測手段、２は等角
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、７は第２の遅延要素、８は等角速度運動に基づ
く平滑手段、９は等速直線運動に基づく平滑手段、１０
は残差による予測値統合手段、１３は第３の遅延要素、
１４は目標位置情報評価手段、１５は信頼度算出手段、
１９は目標存在期待領域統合手段である。
速度運動に基づく予測手段、３は第１の角速度算出手
段、４は第１の遅延要素、５は等速直線運動に基づく予
測手段、７は第２の遅延要素、８は等角速度運動に基づ
く平滑手段、９は等速直線運動に基づく平滑手段、１０
は残差による予測値統合手段、１３は第３の遅延要素、
１４は目標位置情報評価手段、１５は信頼度算出手段、
１９は目標存在期待領域統合手段である。
【０２２２】目標存在期待領域統合手段１９は、等角速
度運動に基づく予測手段２で算出される予測値ベクトル
「−ｘk (a)（−）」から上記の式６８を用いて変換した
予測観測値「−ｚk (a)（−）」、等速直線運動に基づく
予測手段５で算出される予測値ベクトル「−ｘ
k (v)（−）」から上記の式６９を用いて変換した予測観
測値「−ｚk (v)（−）」、等角速度運動に基づく予測手
段２で算出される予測誤差共分散行列「Ｐk (a)（−）」
から上記の式７０を用いて変換した予測観測値の誤差共
分散行列「Ｓk (a)（−）」、等速直線運動に基づく予測
手段５で算出される予測誤差共分散行列「Ｐ
k (v)（−）」から上記の式７１を用いて変換した予測観
測値の誤差共分散行列「Ｓk (v)（−）」を用いて、上記
の式７２、式７３のようにそれぞれ表される等角速度運
動に基づく目標存在期待領域、等速直線運動に基づく目
標存在期待領域を、下記の式７４を用いて予測値と観測
値の残差で重み付け統合を行なう。統合した予測観測値
の誤差共分散行列「Ｓk（−）」と上記の式６６から算
出される統合した予測値ベクトル「−ｘk（−）」から
下記の式７５を用いて算出する。統合した予測観測値ベ
クトル「−ｚk（−）」を用いて、下記の式７６で表さ
れる統合した目標存在期待領域を算出する。なお、図１
０と同一の構成部分には、同一符号を付して説明を省略
する。
度運動に基づく予測手段２で算出される予測値ベクトル
「−ｘk (a)（−）」から上記の式６８を用いて変換した
予測観測値「−ｚk (a)（−）」、等速直線運動に基づく
予測手段５で算出される予測値ベクトル「−ｘ
k (v)（−）」から上記の式６９を用いて変換した予測観
測値「−ｚk (v)（−）」、等角速度運動に基づく予測手
段２で算出される予測誤差共分散行列「Ｐk (a)（−）」
から上記の式７０を用いて変換した予測観測値の誤差共
分散行列「Ｓk (a)（−）」、等速直線運動に基づく予測
手段５で算出される予測誤差共分散行列「Ｐ
k (v)（−）」から上記の式７１を用いて変換した予測観
測値の誤差共分散行列「Ｓk (v)（−）」を用いて、上記
の式７２、式７３のようにそれぞれ表される等角速度運
動に基づく目標存在期待領域、等速直線運動に基づく目
標存在期待領域を、下記の式７４を用いて予測値と観測
値の残差で重み付け統合を行なう。統合した予測観測値
の誤差共分散行列「Ｓk（−）」と上記の式６６から算
出される統合した予測値ベクトル「−ｘk（−）」から
下記の式７５を用いて算出する。統合した予測観測値ベ
クトル「−ｚk（−）」を用いて、下記の式７６で表さ
れる統合した目標存在期待領域を算出する。なお、図１
０と同一の構成部分には、同一符号を付して説明を省略
する。
【０２２３】
【数７４】
【０２２４】
【数７５】
【０２２５】
【数７６】
【０２２６】次に、この実施の形態８に係る追尾装置の
動作について図面を参照しながら説明する。図２０は、
この発明の実施の形態８に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態４と同一動
作部分には同一符号を付して説明を省略する。
動作について図面を参照しながら説明する。図２０は、
この発明の実施の形態８に係る追尾装置の動作を示すフ
ローチャートである。なお、上記実施の形態４と同一動
作部分には同一符号を付して説明を省略する。
【０２２７】ステップＳＴ１１ａの目標存在期待領域統
合処理において、ステップＳＴ９で算出された等角速度
運動に基づく目標存在期待領域、ステップＳＴ１０で算
出された等速直線運動に基づく目標存在期待領域より、
それらを予測値と観測値の残差で重み付け統合して、統
合された目標存在期待領域を算出する。
合処理において、ステップＳＴ９で算出された等角速度
運動に基づく目標存在期待領域、ステップＳＴ１０で算
出された等速直線運動に基づく目標存在期待領域より、
それらを予測値と観測値の残差で重み付け統合して、統
合された目標存在期待領域を算出する。
【０２２８】従って、この実施の形態８によれば、等角
速度運動に基づく予測誤差共分散行列および等速直線運
動に基づく予測誤差共分散行列を予測値と観測値の残差
で統合することにより、目標の運動状態を反映した目標
存在期待領域を設定することができ、観測値ベクトルを
目標存在期待領域内に存在するか否かを判定し、信頼度
を算出することによって、不要信号の無い自由空間のみ
ならず、不要信号環境下においても追従性および追尾精
度が確保できる。
速度運動に基づく予測誤差共分散行列および等速直線運
動に基づく予測誤差共分散行列を予測値と観測値の残差
で統合することにより、目標の運動状態を反映した目標
存在期待領域を設定することができ、観測値ベクトルを
目標存在期待領域内に存在するか否かを判定し、信頼度
を算出することによって、不要信号の無い自由空間のみ
ならず、不要信号環境下においても追従性および追尾精
度が確保できる。
【０２２９】
【発明の効果】この発明に係る追尾装置は、以上説明し
たとおり、極座標において目標位置情報を観測し、その
目標位置情報を固定直交座標に変換し観測値として出力
する観測手段と、固定直交座標において、現時刻より１
サンプリング後の等角速度運動に基づく状態ベクトルの
状態遷移行列、並びに現時刻より１サンプリング後の目
標の運動諸元の第１の予測値、及び第１の予測誤差共分
散行列を算出する等角速度運動に基づく予測手段と、目
標の位置推移に無関係な速度ベクトル方向の角速度を
０、目標の速度ベクトルを回転させる角速度を一定、及
び目標進行方向の速度を一定とするという前提条件を用
いて、目標の速度・加速度より、固定直交座標において
角速度を算出し、さらに、目標位置ベクトルの３次微分
を角速度ベクトルの大きさと目標位置ベクトルの１次微
分から算出する角速度算出手段と、前記等角速度運動に
基づく予測手段で算出する前記状態遷移行列を算出する
ために、１サンプリング前に、前記角速度算出手段で算
出した角速度を入力する第１の遅延要素と、固定直交座
標において、現時刻より１サンプリング後の目標の運動
諸元の第２の予測値、及び第２の予測誤差共分散行列を
算出する等速直線運動に基づく予測手段と、固定直交座
標において、前記等角速度運動に基づく予測手段により
得られた第１の予測値、及び第１の予測誤差共分散行
列、並びに前記等速直線運動に基づく予測手段により得
られた第２の予測値、及び第２の予測誤差共分散行列の
内、前記第１の予測値又は前記第２の予測値と前記観測
手段によって得られる前記観測値との残差が最小になる
ものを１サンプリング後の第３の予測値、及び第３の予
測誤差共分散行列とする残差による予測値選択手段と、
１サンプリング前に、前記残差による予測値選択手段に
よって得られた前記第３の予測値、及び第３の予測誤差
共分散行列を出力する第２の遅延要素と、固定直交座標
において、前記第２の遅延要素より得られる第３の予測
値、及び第３の予測誤差共分散行列、並びに前記観測手
段により得られる観測値より、第１の平滑値、及び第１
の平滑誤差共分散行列を算出する等角速度運動に基づく
平滑手段と、固定直交座標において、前記第２の遅延要
素より得られる第３の予測値、及び第３の予測誤差共分
散行列、並びに前記観測手段により得られる観測値よ
り、第２の平滑値、及び第２の平滑誤差共分散行列を算
出する等速直線運動に基づく平滑手段とを備えたので、
旋回目標及び直進目標に対しても精度良く追尾でき、固
定直交座標−動的直交座標間で座標変換を不要としてア
ルゴリズムを簡略化することができるという効果を奏す
る。
たとおり、極座標において目標位置情報を観測し、その
目標位置情報を固定直交座標に変換し観測値として出力
する観測手段と、固定直交座標において、現時刻より１
サンプリング後の等角速度運動に基づく状態ベクトルの
状態遷移行列、並びに現時刻より１サンプリング後の目
標の運動諸元の第１の予測値、及び第１の予測誤差共分
散行列を算出する等角速度運動に基づく予測手段と、目
標の位置推移に無関係な速度ベクトル方向の角速度を
０、目標の速度ベクトルを回転させる角速度を一定、及
び目標進行方向の速度を一定とするという前提条件を用
いて、目標の速度・加速度より、固定直交座標において
角速度を算出し、さらに、目標位置ベクトルの３次微分
を角速度ベクトルの大きさと目標位置ベクトルの１次微
分から算出する角速度算出手段と、前記等角速度運動に
基づく予測手段で算出する前記状態遷移行列を算出する
ために、１サンプリング前に、前記角速度算出手段で算
出した角速度を入力する第１の遅延要素と、固定直交座
標において、現時刻より１サンプリング後の目標の運動
諸元の第２の予測値、及び第２の予測誤差共分散行列を
算出する等速直線運動に基づく予測手段と、固定直交座
標において、前記等角速度運動に基づく予測手段により
得られた第１の予測値、及び第１の予測誤差共分散行
列、並びに前記等速直線運動に基づく予測手段により得
られた第２の予測値、及び第２の予測誤差共分散行列の
内、前記第１の予測値又は前記第２の予測値と前記観測
手段によって得られる前記観測値との残差が最小になる
ものを１サンプリング後の第３の予測値、及び第３の予
測誤差共分散行列とする残差による予測値選択手段と、
１サンプリング前に、前記残差による予測値選択手段に
よって得られた前記第３の予測値、及び第３の予測誤差
共分散行列を出力する第２の遅延要素と、固定直交座標
において、前記第２の遅延要素より得られる第３の予測
値、及び第３の予測誤差共分散行列、並びに前記観測手
段により得られる観測値より、第１の平滑値、及び第１
の平滑誤差共分散行列を算出する等角速度運動に基づく
平滑手段と、固定直交座標において、前記第２の遅延要
素より得られる第３の予測値、及び第３の予測誤差共分
散行列、並びに前記観測手段により得られる観測値よ
り、第２の平滑値、及び第２の平滑誤差共分散行列を算
出する等速直線運動に基づく平滑手段とを備えたので、
旋回目標及び直進目標に対しても精度良く追尾でき、固
定直交座標−動的直交座標間で座標変換を不要としてア
ルゴリズムを簡略化することができるという効果を奏す
る。
【０２３０】また、この発明に係る追尾装置は、以上説
明したとおり、さらに、前記等角速度運動に基づく予測
手段より得られる第１の予測値、及び第１の予測誤差共
分散行列から算出される第１の目標存在期待領域、前記
等速直線運動に基づく予測手段で算出される予測値ベク
トル、及び第２の予測誤差共分散行列から算出される第
２の目標存在期待領域の内、目標位置に関する予測誤差
共分散行列が最大となる第３の目標存在期待領域を選択
する目標存在期待領域選択手段と、第３の遅延要素を通
して入力された前記目標存在期待領域選択手段により選
択された前記第３の目標存在期待領域を用いて、前記観
測手段により求められる前記観測値が前記第３の目標存
在期待領域内に存在するか否かを判定する目標位置情報
評価手段と、前記第３の目標存在期待領域内に存在する
観測値の内、前記第３の目標存在期待領域内の中心から
の距離に応じて算出される確率密度により信頼度を算出
する信頼度算出手段とを備え、前記等角速度運動に基づ
く平滑手段は、前記信頼度により、前記第３の予測値、
及び前記第３の予測誤差共分散行列、並びに前記観測手
段により得られる観測値の重み付けを行い、第１の平滑
値、及び第１の平滑誤差共分散行列を算出し、前記等速
直線運動に基づく平滑手段は、前記信頼度により、前記
第３の予測値、及び前記第３の予測誤差共分散行列、並
びに前記観測手段により得られる観測値の重み付けを行
い、第２の平滑値、及び第２の平滑誤差共分散行列を算
出するので、旋回目標及び直進目標に対しても精度良く
追尾でき、固定直交座標−動的直交座標間で座標変換を
不要としてアルゴリズムを簡略化することができるとい
う効果を奏する。
明したとおり、さらに、前記等角速度運動に基づく予測
手段より得られる第１の予測値、及び第１の予測誤差共
分散行列から算出される第１の目標存在期待領域、前記
等速直線運動に基づく予測手段で算出される予測値ベク
トル、及び第２の予測誤差共分散行列から算出される第
２の目標存在期待領域の内、目標位置に関する予測誤差
共分散行列が最大となる第３の目標存在期待領域を選択
する目標存在期待領域選択手段と、第３の遅延要素を通
して入力された前記目標存在期待領域選択手段により選
択された前記第３の目標存在期待領域を用いて、前記観
測手段により求められる前記観測値が前記第３の目標存
在期待領域内に存在するか否かを判定する目標位置情報
評価手段と、前記第３の目標存在期待領域内に存在する
観測値の内、前記第３の目標存在期待領域内の中心から
の距離に応じて算出される確率密度により信頼度を算出
する信頼度算出手段とを備え、前記等角速度運動に基づ
く平滑手段は、前記信頼度により、前記第３の予測値、
及び前記第３の予測誤差共分散行列、並びに前記観測手
段により得られる観測値の重み付けを行い、第１の平滑
値、及び第１の平滑誤差共分散行列を算出し、前記等速
直線運動に基づく平滑手段は、前記信頼度により、前記
第３の予測値、及び前記第３の予測誤差共分散行列、並
びに前記観測手段により得られる観測値の重み付けを行
い、第２の平滑値、及び第２の平滑誤差共分散行列を算
出するので、旋回目標及び直進目標に対しても精度良く
追尾でき、固定直交座標−動的直交座標間で座標変換を
不要としてアルゴリズムを簡略化することができるとい
う効果を奏する。
【０２３１】また、この発明に係る追尾装置は、以上説
明したとおり、さらに、前記等角速度運動に基づく予測
手段から得られる第１の予測値、及び第１の予測誤差共
分散行列、前記等速直線運動に基づく予測手段から得ら
れる第２の予測値、及び第２の予測誤差共分散行列、並
びに前記観測手段から求まる観測値を元に、前記第１又
は第２の予測値と前記観測値の残差で、目標の運動状態
を判定してサンプリングレートを変える残差によるサン
プリング制御手段を備えたので、旋回目標及び直進目標
に対しても精度良く追尾でき、固定直交座標−動的直交
座標間で座標変換を不要としてアルゴリズムを簡略化す
ることができるという効果を奏する。
明したとおり、さらに、前記等角速度運動に基づく予測
手段から得られる第１の予測値、及び第１の予測誤差共
分散行列、前記等速直線運動に基づく予測手段から得ら
れる第２の予測値、及び第２の予測誤差共分散行列、並
びに前記観測手段から求まる観測値を元に、前記第１又
は第２の予測値と前記観測値の残差で、目標の運動状態
を判定してサンプリングレートを変える残差によるサン
プリング制御手段を備えたので、旋回目標及び直進目標
に対しても精度良く追尾でき、固定直交座標−動的直交
座標間で座標変換を不要としてアルゴリズムを簡略化す
ることができるという効果を奏する。
【０２３２】また、この発明に係る追尾装置は、以上説
明したとおり、さらに、前記角速度算出手段より求まる
角速度により目標の運動状態を判定してサンプリングレ
ートを変える角速度によるサンプリング制御手段を備え
たので、旋回目標及び直進目標に対しても精度良く追尾
でき、固定直交座標−動的直交座標間で座標変換を不要
としてアルゴリズムを簡略化することができるという効
果を奏する。
明したとおり、さらに、前記角速度算出手段より求まる
角速度により目標の運動状態を判定してサンプリングレ
ートを変える角速度によるサンプリング制御手段を備え
たので、旋回目標及び直進目標に対しても精度良く追尾
でき、固定直交座標−動的直交座標間で座標変換を不要
としてアルゴリズムを簡略化することができるという効
果を奏する。
【０２３３】また、この発明に係る追尾装置は、以上説
明したとおり、さらに、前記等角速度運動に基づく予測
手段から得られる第１の予測値、前記等速直線運動に基
づく予測手段から得られる第２の予測値から、目標軌跡
に一番近いと考えられる前記第１又は第２の予測位置の
１つのみを表示する表示手段を備えたので、旋回目標及
び直進目標に対しても精度良く追尾でき、固定直交座標
−動的直交座標間で座標変換を不要としてアルゴリズム
を簡略化することができるという効果を奏する。
明したとおり、さらに、前記等角速度運動に基づく予測
手段から得られる第１の予測値、前記等速直線運動に基
づく予測手段から得られる第２の予測値から、目標軌跡
に一番近いと考えられる前記第１又は第２の予測位置の
１つのみを表示する表示手段を備えたので、旋回目標及
び直進目標に対しても精度良く追尾でき、固定直交座標
−動的直交座標間で座標変換を不要としてアルゴリズム
を簡略化することができるという効果を奏する。
【０２３４】この発明に係る追尾装置は、以上説明した
とおり、極座標において目標位置情報を観測し、その目
標位置情報を固定直交座標に変換し観測値として出力す
る観測手段と、固定直交座標において、現時刻より１サ
ンプリング後の等角速度運動に基づく状態ベクトルの状
態遷移行列、並びに現時刻より１サンプリング後の目標
の運動諸元の第１の予測値、及び第１の予測誤差共分散
行列を算出する等角速度運動に基づく予測手段と、目標
の位置推移に無関係な速度ベクトル方向の角速度を０、
目標の速度ベクトルを回転させる角速度を一定、及び目
標進行方向の速度を一定とするという前提条件を用い
て、目標の速度・加速度より、固定直交座標において角
速度を算出し、さらに、目標位置ベクトルの３次微分を
角速度ベクトルの大きさと目標位置ベクトルの１次微分
から算出する角速度算出手段と、前記等角速度運動に基
づく予測手段で算出する前記状態遷移行列を算出するた
めに、１サンプリング前に、前記角速度算出手段で算出
した角速度を入力する第１の遅延要素と、固定直交座標
において、現時刻より１サンプリング後の目標の運動諸
元の第２の予測値、及び第２の予測誤差共分散行列を算
出する等速直線運動に基づく予測手段と、固定直交座標
において、前記等角速度運動に基づく予測手段により得
られた第１の予測値、及び第１の予測誤差共分散行列、
前記等速直線運動に基づく予測手段により得られた第２
の予測値、及び第２の予測誤差共分散行列、並びに前記
観測手段により得られた観測値を元に、前記第１又は第
２の予測値と前記観測値の残差で、前記等角速度運動に
基づく第１の予測値、及び第１の予測誤差共分散行列と
前記等速直線運動に基づく第２の予測値、及び第２の予
測誤差共分散行列を重み付けしたものを第３の予測値、
及び第３の予測誤差共分散行列とする残差による予測値
統合手段と、１サンプリング前に、前記残差による予測
値統合手段によって得られた前記第３の予測値、及び第
３の予測誤差共分散行列を出力する第２の遅延要素と、
固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第１の平滑
値、及び第１の平滑誤差共分散行列を算出する等角速度
運動に基づく平滑手段と、固定直交座標において、前記
第２の遅延要素より得られる第３の予測値、及び第３の
予測誤差共分散行列、並びに前記観測手段により得られ
る観測値より、第２の平滑値、及び第２の平滑誤差共分
散行列を算出する等速直線運動に基づく平滑手段とを備
えたので、旋回目標及び直進目標に対しても精度良く追
尾でき、固定直交座標−動的直交座標間で座標変換を不
要としてアルゴリズムを簡略化することができるという
効果を奏する。
とおり、極座標において目標位置情報を観測し、その目
標位置情報を固定直交座標に変換し観測値として出力す
る観測手段と、固定直交座標において、現時刻より１サ
ンプリング後の等角速度運動に基づく状態ベクトルの状
態遷移行列、並びに現時刻より１サンプリング後の目標
の運動諸元の第１の予測値、及び第１の予測誤差共分散
行列を算出する等角速度運動に基づく予測手段と、目標
の位置推移に無関係な速度ベクトル方向の角速度を０、
目標の速度ベクトルを回転させる角速度を一定、及び目
標進行方向の速度を一定とするという前提条件を用い
て、目標の速度・加速度より、固定直交座標において角
速度を算出し、さらに、目標位置ベクトルの３次微分を
角速度ベクトルの大きさと目標位置ベクトルの１次微分
から算出する角速度算出手段と、前記等角速度運動に基
づく予測手段で算出する前記状態遷移行列を算出するた
めに、１サンプリング前に、前記角速度算出手段で算出
した角速度を入力する第１の遅延要素と、固定直交座標
において、現時刻より１サンプリング後の目標の運動諸
元の第２の予測値、及び第２の予測誤差共分散行列を算
出する等速直線運動に基づく予測手段と、固定直交座標
において、前記等角速度運動に基づく予測手段により得
られた第１の予測値、及び第１の予測誤差共分散行列、
前記等速直線運動に基づく予測手段により得られた第２
の予測値、及び第２の予測誤差共分散行列、並びに前記
観測手段により得られた観測値を元に、前記第１又は第
２の予測値と前記観測値の残差で、前記等角速度運動に
基づく第１の予測値、及び第１の予測誤差共分散行列と
前記等速直線運動に基づく第２の予測値、及び第２の予
測誤差共分散行列を重み付けしたものを第３の予測値、
及び第３の予測誤差共分散行列とする残差による予測値
統合手段と、１サンプリング前に、前記残差による予測
値統合手段によって得られた前記第３の予測値、及び第
３の予測誤差共分散行列を出力する第２の遅延要素と、
固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第１の平滑
値、及び第１の平滑誤差共分散行列を算出する等角速度
運動に基づく平滑手段と、固定直交座標において、前記
第２の遅延要素より得られる第３の予測値、及び第３の
予測誤差共分散行列、並びに前記観測手段により得られ
る観測値より、第２の平滑値、及び第２の平滑誤差共分
散行列を算出する等速直線運動に基づく平滑手段とを備
えたので、旋回目標及び直進目標に対しても精度良く追
尾でき、固定直交座標−動的直交座標間で座標変換を不
要としてアルゴリズムを簡略化することができるという
効果を奏する。
【０２３５】また、この発明に係る追尾装置は、以上説
明したとおり、さらに、前記等角速度運動に基づく予測
手段より得られる第１の予測値、及び第１の予測誤差共
分散行列から算出される第１の目標存在期待領域、前記
等速直線運動に基づく予測手段で算出される第２の予測
値、及び第２の予測誤差共分散行列から算出される第２
の目標存在期待領域の内、前記等速直線運動に基づく目
標位置に関する第２の予測誤差共分散行列と前記等角速
度運動に基づく目標位置に関する第１の予測誤差共分散
行列を前記第１又は第２の予測値と観測値の残差で重み
付けをして統合を行ない、その統合された第３の予測誤
差共分散行列を用いて、統合された第３の目標存在期待
領域を算出する目標存在期待領域統合手段と、第３の遅
延要素を通して入力された前記目標存在期待領域統合手
段により統合された前記第３の目標存在期待領域を用い
て、前記観測手段により求められる前記観測値が前記第
３の目標存在期待領域内に存在するか否かを判定する目
標位置情報評価手段と、前記第３の目標存在期待領域内
に存在する観測値の内、前記第３の目標存在期待領域内
の中心からの距離に応じて算出される確率密度により信
頼度を算出する信頼度算出手段とを備え、前記等角速度
運動に基づく平滑手段は、前記信頼度により、前記第３
の予測値、及び前記第３の予測誤差共分散行列、並びに
前記観測手段により得られる観測値の重み付けを行い、
第１の平滑値、及び第１の平滑誤差共分散行列を算出
し、前記等速直線運動に基づく平滑手段は、前記信頼度
により、前記第３の予測値、及び前記第３の予測誤差共
分散行列、並びに前記観測手段により得られる観測値の
重み付けを行い、第２の平滑値、及び第２の平滑誤差共
分散行列を算出するので、旋回目標及び直進目標に対し
ても精度良く追尾でき、固定直交座標−動的直交座標間
で座標変換を不要としてアルゴリズムを簡略化すること
ができるという効果を奏する。
明したとおり、さらに、前記等角速度運動に基づく予測
手段より得られる第１の予測値、及び第１の予測誤差共
分散行列から算出される第１の目標存在期待領域、前記
等速直線運動に基づく予測手段で算出される第２の予測
値、及び第２の予測誤差共分散行列から算出される第２
の目標存在期待領域の内、前記等速直線運動に基づく目
標位置に関する第２の予測誤差共分散行列と前記等角速
度運動に基づく目標位置に関する第１の予測誤差共分散
行列を前記第１又は第２の予測値と観測値の残差で重み
付けをして統合を行ない、その統合された第３の予測誤
差共分散行列を用いて、統合された第３の目標存在期待
領域を算出する目標存在期待領域統合手段と、第３の遅
延要素を通して入力された前記目標存在期待領域統合手
段により統合された前記第３の目標存在期待領域を用い
て、前記観測手段により求められる前記観測値が前記第
３の目標存在期待領域内に存在するか否かを判定する目
標位置情報評価手段と、前記第３の目標存在期待領域内
に存在する観測値の内、前記第３の目標存在期待領域内
の中心からの距離に応じて算出される確率密度により信
頼度を算出する信頼度算出手段とを備え、前記等角速度
運動に基づく平滑手段は、前記信頼度により、前記第３
の予測値、及び前記第３の予測誤差共分散行列、並びに
前記観測手段により得られる観測値の重み付けを行い、
第１の平滑値、及び第１の平滑誤差共分散行列を算出
し、前記等速直線運動に基づく平滑手段は、前記信頼度
により、前記第３の予測値、及び前記第３の予測誤差共
分散行列、並びに前記観測手段により得られる観測値の
重み付けを行い、第２の平滑値、及び第２の平滑誤差共
分散行列を算出するので、旋回目標及び直進目標に対し
ても精度良く追尾でき、固定直交座標−動的直交座標間
で座標変換を不要としてアルゴリズムを簡略化すること
ができるという効果を奏する。
【０２３６】この発明に係る追尾装置は、以上説明した
とおり、極座標において目標位置情報を観測し、その目
標位置情報を固定直交座標に変換し観測値として出力す
る観測手段と、固定直交座標において、現時刻より１サ
ンプリング後の等角速度運動に基づく状態ベクトルの状
態遷移行列、並びに現時刻より１サンプリング後の目標
の運動諸元の第１の予測値、及び第１の予測誤差共分散
行列を算出する等角速度運動に基づく予測手段と、目標
の位置推移に無関係な速度ベクトル方向の角速度を０、
目標の速度ベクトルを回転させる角速度を一定、及び目
標進行方向の速度を一定とするという前提条件を用い
て、目標の速度・加速度より、固定直交座標において角
速度を算出し、さらに、目標位置ベクトルの３次微分を
角速度ベクトルの大きさと目標位置ベクトルの１次微分
から算出する角速度算出手段と、前記等角速度運動に基
づく予測手段で算出する前記状態遷移行列を算出するた
めに、１サンプリング前に、前記角速度算出手段で算出
した角速度を入力する第１の遅延要素と、固定直交座標
において、現時刻より１サンプリング後の目標の運動諸
元の第２の予測値、及び第２の予測誤差共分散行列を算
出する等速直線運動に基づく予測手段と、固定直交座標
において、前記等角速度運動に基づく予測手段により得
られた第１の予測値、及び第１の予測誤差共分散行列、
並びに前記等速直線運動に基づく予測手段により得られ
た第２の予測値、及び第２の予測誤差共分散行列を、前
記角速度算出手段より求まる角速度の大きさにより選択
する角速度による予測値選択手段と、１サンプリング前
に、前記角速度による予測値選択手段によって得られた
第３の予測値、及び第３の予測誤差共分散行列を出力す
る第２の遅延要素と、固定直交座標において、前記第２
の遅延要素より得られる第３の予測値、及び第３の予測
誤差共分散行列、並びに前記観測手段により得られる観
測値より、第１の平滑値、及び第１の平滑誤差共分散行
列を算出する等角速度運動に基づく平滑手段と、固定直
交座標において、前記第２の遅延要素より得られる第３
の予測値、及び第３の予測誤差共分散行列、並びに前記
観測手段により得られる観測値より、第２の平滑値、及
び第２の平滑誤差共分散行列を算出する等速直線運動に
基づく平滑手段とを備えたので、旋回目標及び直進目標
に対しても精度良く追尾でき、固定直交座標−動的直交
座標間で座標変換を不要としてアルゴリズムを簡略化す
ることができるという効果を奏する。
とおり、極座標において目標位置情報を観測し、その目
標位置情報を固定直交座標に変換し観測値として出力す
る観測手段と、固定直交座標において、現時刻より１サ
ンプリング後の等角速度運動に基づく状態ベクトルの状
態遷移行列、並びに現時刻より１サンプリング後の目標
の運動諸元の第１の予測値、及び第１の予測誤差共分散
行列を算出する等角速度運動に基づく予測手段と、目標
の位置推移に無関係な速度ベクトル方向の角速度を０、
目標の速度ベクトルを回転させる角速度を一定、及び目
標進行方向の速度を一定とするという前提条件を用い
て、目標の速度・加速度より、固定直交座標において角
速度を算出し、さらに、目標位置ベクトルの３次微分を
角速度ベクトルの大きさと目標位置ベクトルの１次微分
から算出する角速度算出手段と、前記等角速度運動に基
づく予測手段で算出する前記状態遷移行列を算出するた
めに、１サンプリング前に、前記角速度算出手段で算出
した角速度を入力する第１の遅延要素と、固定直交座標
において、現時刻より１サンプリング後の目標の運動諸
元の第２の予測値、及び第２の予測誤差共分散行列を算
出する等速直線運動に基づく予測手段と、固定直交座標
において、前記等角速度運動に基づく予測手段により得
られた第１の予測値、及び第１の予測誤差共分散行列、
並びに前記等速直線運動に基づく予測手段により得られ
た第２の予測値、及び第２の予測誤差共分散行列を、前
記角速度算出手段より求まる角速度の大きさにより選択
する角速度による予測値選択手段と、１サンプリング前
に、前記角速度による予測値選択手段によって得られた
第３の予測値、及び第３の予測誤差共分散行列を出力す
る第２の遅延要素と、固定直交座標において、前記第２
の遅延要素より得られる第３の予測値、及び第３の予測
誤差共分散行列、並びに前記観測手段により得られる観
測値より、第１の平滑値、及び第１の平滑誤差共分散行
列を算出する等角速度運動に基づく平滑手段と、固定直
交座標において、前記第２の遅延要素より得られる第３
の予測値、及び第３の予測誤差共分散行列、並びに前記
観測手段により得られる観測値より、第２の平滑値、及
び第２の平滑誤差共分散行列を算出する等速直線運動に
基づく平滑手段とを備えたので、旋回目標及び直進目標
に対しても精度良く追尾でき、固定直交座標−動的直交
座標間で座標変換を不要としてアルゴリズムを簡略化す
ることができるという効果を奏する。
【図１】 この発明の実施の形態１に係る追尾装置の構
成を示すブロック図である。
成を示すブロック図である。
【図２】 この発明の実施の形態１に係る固定直交座標
系を示す図である。
系を示す図である。
【図３】 この発明の実施の形態１に係る動的直交座標
系を示す図である。
系を示す図である。
【図４】 この発明の実施の形態１に係る動的直交座標
系の目標姿勢角を示す図である。
系の目標姿勢角を示す図である。
【図５】 この発明の実施の形態１に係る追尾装置の動
作を示すフローチャートである。
作を示すフローチャートである。
【図６】 この発明の実施の形態２に係る追尾装置の構
成を示すブロック図である。
成を示すブロック図である。
【図７】 この発明の実施の形態２に係る追尾装置の動
作を示すフローチャートである。
作を示すフローチャートである。
【図８】 この発明の実施の形態３に係る追尾装置の構
成を示すブロック図である。
成を示すブロック図である。
【図９】 この発明の実施の形態３に係る追尾装置の動
作を示すフローチャートである。
作を示すフローチャートである。
【図１０】 この発明の実施の形態４に係る追尾装置の
構成を示すブロック図である。
構成を示すブロック図である。
【図１１】 この発明の実施の形態４に係る目標存在期
待領域と信頼度の関係を表す図である。
待領域と信頼度の関係を表す図である。
【図１２】 この発明の実施の形態４に係る追尾装置の
動作を示すフローチャートである。
動作を示すフローチャートである。
【図１３】 この発明の実施の形態５に係る追尾装置の
構成を示すブロック図である。
構成を示すブロック図である。
【図１４】 この発明の実施の形態５に係る追尾装置の
動作を示すフローチャートである。
動作を示すフローチャートである。
【図１５】 この発明の実施の形態６に係る追尾装置の
構成を示すブロック図である。
構成を示すブロック図である。
【図１６】 この発明の実施の形態６に係る追尾装置の
動作を示すフローチャートである。
動作を示すフローチャートである。
【図１７】 この発明の実施の形態７に係る追尾装置の
構成を示すブロック図である。
構成を示すブロック図である。
【図１８】 この発明の実施の形態７に係る追尾装置の
動作を示すフローチャートである。
動作を示すフローチャートである。
【図１９】 この発明の実施の形態８に係る追尾装置の
構成を示すブロック図である。
構成を示すブロック図である。
【図２０】 この発明の実施の形態８に係る追尾装置の
動作を示すフローチャートである。
動作を示すフローチャートである。
【図２１】 従来の追尾装置の構成を示すブロック図で
ある。
ある。
１ 観測手段、２ 等角速度運動に基づく予測手段、３
第１の角速度算出手段、４ 第１の遅延要素、５ 等速
直線運動に基づく予測手段、６ 残差による予測値選択
手段、７ 第２の遅延要素、８ 等角速度運動に基づく
平滑手段、９等速直線運動に基づく平滑手段、１０ 残
差による予測値統合手段、１１ 角速度による予測値選
択手段、１２ 目標存在期待領域選択手段、１３ 第３
の遅延要素、１４ 目標位置情報評価手段、１５ 信頼
度算出手段、１６ 残差によるサンプリング制御手段、
１７ 角速度によるサンプリング制御手段、１８ 表示
手段、１９ 目標存在期待領域統合手段。
第１の角速度算出手段、４ 第１の遅延要素、５ 等速
直線運動に基づく予測手段、６ 残差による予測値選択
手段、７ 第２の遅延要素、８ 等角速度運動に基づく
平滑手段、９等速直線運動に基づく平滑手段、１０ 残
差による予測値統合手段、１１ 角速度による予測値選
択手段、１２ 目標存在期待領域選択手段、１３ 第３
の遅延要素、１４ 目標位置情報評価手段、１５ 信頼
度算出手段、１６ 残差によるサンプリング制御手段、
１７ 角速度によるサンプリング制御手段、１８ 表示
手段、１９ 目標存在期待領域統合手段。
───────────────────────────────────────────────────── フロントページの続き 審査官 宮川 哲伸 (56)参考文献 特開 平５−288840（ＪＰ，Ａ) 特開 平９−96500（ＪＰ，Ａ) 特開 平２−53317（ＪＰ，Ａ) 特開 平８−262128（ＪＰ，Ａ) 特開 平９−90027（ＪＰ，Ａ) 特開 平９−171072（ＪＰ，Ａ) (58)調査した分野(Int.Cl.7，ＤＢ名) G01S 7/00 - 7/42 G01S 13/00 - 13/95
Claims (8)
- 【請求項１】 極座標において目標位置情報を観測し、
その目標位置情報を固定直交座標に変換し観測値として
出力する観測手段と、 固定直交座標において、現時刻より１サンプリング後の
等角速度運動に基づく状態ベクトルの状態遷移行列、並
びに現時刻より１サンプリング後の目標の運動諸元の第
１の予測値、及び第１の予測誤差共分散行列を算出する
等角速度運動に基づく予測手段と、 目標の位置推移に無関係な速度ベクトル方向の角速度を
０、目標の速度ベクトルを回転させる角速度を一定、及
び目標進行方向の速度を一定とするという前提条件を用
いて、目標の速度・加速度より、固定直交座標において
角速度を算出し、さらに、目標位置ベクトルの３次微分
を角速度ベクトルの大きさと目標位置ベクトルの１次微
分から算出する角速度算出手段と、 前記等角速度運動に基づく予測手段で算出する前記状態
遷移行列を算出するために、１サンプリング前に、前記
角速度算出手段で算出した角速度を入力する第１の遅延
要素と、 固定直交座標において、現時刻より１サンプリング後の
目標の運動諸元の第２の予測値、及び第２の予測誤差共
分散行列を算出する等速直線運動に基づく予測手段と、 固定直交座標において、前記等角速度運動に基づく予測
手段により得られた第１の予測値、及び第１の予測誤差
共分散行列、並びに前記等速直線運動に基づく予測手段
により得られた第２の予測値、及び第２の予測誤差共分
散行列の内、前記第１の予測値又は前記第２の予測値と
前記観測手段によって得られる前記観測値との残差が最
小になるものを１サンプリング後の第３の予測値、及び
第３の予測誤差共分散行列とする残差による予測値選択
手段と、 １サンプリング前に、前記残差による予測値選択手段に
よって得られた前記第３の予測値、及び第３の予測誤差
共分散行列を出力する第２の遅延要素と、 固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第１の平滑
値、及び第１の平滑誤差共分散行列を算出する等角速度
運動に基づく平滑手段と、 固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第２の平滑
値、及び第２の平滑誤差共分散行列を算出する等速直線
運動に基づく平滑手段とを備えたことを特徴とする追尾
装置。 - 【請求項２】 さらに、 前記等角速度運動に基づく予測手段より得られる第１の
予測値、及び第１の予測誤差共分散行列から算出される
第１の目標存在期待領域、前記等速直線運動に基づく予
測手段で算出される予測値ベクトル、及び第２の予測誤
差共分散行列から算出される第２の目標存在期待領域の
内、目標位置に関する予測誤差共分散行列が最大となる
第３の目標存在期待領域を選択する目標存在期待領域選
択手段と、 第３の遅延要素を通して入力された前記目標存在期待領
域選択手段により選択された前記第３の目標存在期待領
域を用いて、前記観測手段により求められる前記観測値
が前記第３の目標存在期待領域内に存在するか否かを判
定する目標位置情報評価手段と、 前記第３の目標存在期待領域内に存在する観測値の内、
前記第３の目標存在期待領域内の中心からの距離に応じ
て算出される確率密度により信頼度を算出する信頼度算
出手段とを備え、 前記等角速度運動に基づく平滑手段は、前記信頼度によ
り、前記第３の予測値、及び前記第３の予測誤差共分散
行列、並びに前記観測手段により得られる観測値の重み
付けを行い、第１の平滑値、及び第１の平滑誤差共分散
行列を算出し、 前記等速直線運動に基づく平滑手段は、前記信頼度によ
り、前記第３の予測値、及び前記第３の予測誤差共分散
行列、並びに前記観測手段により得られる観測値の重み
付けを行い、第２の平滑値、及び第２の平滑誤差共分散
行列を算出することを特徴とする請求項１記載の追尾装
置。 - 【請求項３】 さらに、 前記等角速度運動に基づく予測手段から得られる第１の
予測値、及び第１の予測誤差共分散行列、前記等速直線
運動に基づく予測手段から得られる第２の予測値、及び
第２の予測誤差共分散行列、並びに前記観測手段から求
まる観測値を元に、前記第１又は第２の予測値と前記観
測値の残差で、目標の運動状態を判定してサンプリング
レートを変える残差によるサンプリング制御手段を備え
たことを特徴とする請求項１記載の追尾装置。 - 【請求項４】 さらに、 前記角速度算出手段より求まる角速度により目標の運動
状態を判定してサンプリングレートを変える角速度によ
るサンプリング制御手段を備えたことを特徴とする請求
項１記載の追尾装置。 - 【請求項５】 さらに、 前記等角速度運動に基づく予測手段から得られる第１の
予測値、前記等速直線運動に基づく予測手段から得られ
る第２の予測値から、目標軌跡に一番近いと考えられる
前記第１又は第２の予測位置の１つのみを表示する表示
手段を備えたことを特徴とする請求項１記載の追尾装
置。 - 【請求項６】 極座標において目標位置情報を観測し、
その目標位置情報を固定直交座標に変換し観測値として
出力する観測手段と、 固定直交座標において、現時刻より１サンプリング後の
等角速度運動に基づく状態ベクトルの状態遷移行列、並
びに現時刻より１サンプリング後の目標の運動諸元の第
１の予測値、及び第１の予測誤差共分散行列を算出する
等角速度運動に基づく予測手段と、 目標の位置推移に無関係な速度ベクトル方向の角速度を
０、目標の速度ベクトルを回転させる角速度を一定、及
び目標進行方向の速度を一定とするという前提条件を用
いて、目標の速度・加速度より、固定直交座標において
角速度を算出し、さらに、目標位置ベクトルの３次微分
を角速度ベクトルの大きさと目標位置ベクトルの１次微
分から算出する角速度算出手段と、 前記等角速度運動に基づく予測手段で算出する前記状態
遷移行列を算出するために、１サンプリング前に、前記
角速度算出手段で算出した角速度を入力する第１の遅延
要素と、 固定直交座標において、現時刻より１サンプリング後の
目標の運動諸元の第２の予測値、及び第２の予測誤差共
分散行列を算出する等速直線運動に基づく予測手段と、 固定直交座標において、前記等角速度運動に基づく予測
手段により得られた第１の予測値、及び第１の予測誤差
共分散行列、前記等速直線運動に基づく予測手段により
得られた第２の予測値、及び第２の予測誤差共分散行
列、並びに前記観測手段により得られた観測値を元に、
前記第１又は第２の予測値と前記観測値の残差で、前記
等角速度運動に基づく第１の予測値、及び第１の予測誤
差共分散行列と前記等速直線運動に基づく第２の予測
値、及び第２の予測誤差共分散行列を重み付けしたもの
を第３の予測値、及び第３の予測誤差共分散行列とする
残差による予測値統合手段と、 １サンプリング前に、前記残差による予測値統合手段に
よって得られた前記第３の予測値、及び第３の予測誤差
共分散行列を出力する第２の遅延要素と、 固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第１の平滑
値、及び第１の平滑誤差共分散行列を算出する等角速度
運動に基づく平滑手段と、 固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第２の平滑
値、及び第２の平滑誤差共分散行列を算出する等速直線
運動に基づく平滑手段とを備えたことを特徴とする追尾
装置。 - 【請求項７】 さらに、 前記等角速度運動に基づく予測手段より得られる第１の
予測値、及び第１の予測誤差共分散行列から算出される
第１の目標存在期待領域、前記等速直線運動に基づく予
測手段で算出される第２の予測値、及び第２の予測誤差
共分散行列から算出される第２の目標存在期待領域の
内、前記等速直線運動に基づく目標位置に関する第２の
予測誤差共分散行列と前記等角速度運動に基づく目標位
置に関する第１の予測誤差共分散行列を前記第１又は第
２の予測値と観測値の残差で重み付けをして統合を行な
い、その統合された第３の予測誤差共分散行列を用い
て、統合された第３の目標存在期待領域を算出する目標
存在期待領域統合手段と、 第３の遅延要素を通して入力された前記目標存在期待領
域統合手段により統合された前記第３の目標存在期待領
域を用いて、前記観測手段により求められる前記観測値
が前記第３の目標存在期待領域内に存在するか否かを判
定する目標位置情報評価手段と、 前記第３の目標存在期待領域内に存在する観測値の内、
前記第３の目標存在期待領域内の中心からの距離に応じ
て算出される確率密度により信頼度を算出する信頼度算
出手段とを備え、 前記等角速度運動に基づく平滑手段は、前記信頼度によ
り、前記第３の予測値、及び前記第３の予測誤差共分散
行列、並びに前記観測手段により得られる観測値の重み
付けを行い、第１の平滑値、及び第１の平滑誤差共分散
行列を算出し、前記等速直線運動に基づく平滑手段は、
前記信頼度により、前記第３の予測値、及び前記第３の
予測誤差共分散行列、並びに前記観測手段により得られ
る観測値の重み付けを行い、第２の平滑値、及び第２の
平滑誤差共分散行列を算出することを特徴とする請求項
６記載の追尾装置。 - 【請求項８】 極座標において目標位置情報を観測し、
その目標位置情報を固定直交座標に変換し観測値として
出力する観測手段と、 固定直交座標において、現時刻より１サンプリング後の
等角速度運動に基づく状態ベクトルの状態遷移行列、並
びに現時刻より１サンプリング後の目標の運動諸元の第
１の予測値、及び第１の予測誤差共分散行列を算出する
等角速度運動に基づく予測手段と、 目標の位置推移に無関係な速度ベクトル方向の角速度を
０、目標の速度ベクトルを回転させる角速度を一定、及
び目標進行方向の速度を一定とするという前提条件を用
いて、目標の速度・加速度より、固定直交座標において
角速度を算出し、さらに、目標位置ベクトルの３次微分
を角速度ベクトルの大きさと目標位置ベクトルの１次微
分から算出する角速度算出手段と、 前記等角速度運動に基づく予測手段で算出する前記状態
遷移行列を算出するために、１サンプリング前に、前記
角速度算出手段で算出した角速度を入力する第１の遅延
要素と、 固定直交座標において、現時刻より１サンプリング後の
目標の運動諸元の第２の予測値、及び第２の予測誤差共
分散行列を算出する等速直線運動に基づく予測手段と、 固定直交座標において、前記等角速度運動に基づく予測
手段により得られた第１の予測値、及び第１の予測誤差
共分散行列、並びに前記等速直線運動に基づく予測手段
により得られた第２の予測値、及び第２の予測誤差共分
散行列を、前記角速度算出手段より求まる角速度の大き
さにより選択する角速度による予測値選択手段と、 １サンプリング前に、前記角速度による予測値選択手段
によって得られた第３の予測値、及び第３の予測誤差共
分散行列を出力する第２の遅延要素と、 固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第１の平滑
値、及び第１の平滑誤差共分散行列を算出する等角速度
運動に基づく平滑手段と、 固定直交座標において、前記第２の遅延要素より得られ
る第３の予測値、及び第３の予測誤差共分散行列、並び
に前記観測手段により得られる観測値より、第２の平滑
値、及び第２の平滑誤差共分散行列を算出する等速直線
運動に基づく平滑手段とを備えたことを特徴とする追尾
装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP09934998A JP3330872B2 (ja) | 1998-04-10 | 1998-04-10 | 追尾装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP09934998A JP3330872B2 (ja) | 1998-04-10 | 1998-04-10 | 追尾装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH11295417A JPH11295417A (ja) | 1999-10-29 |
| JP3330872B2 true JP3330872B2 (ja) | 2002-09-30 |
Family
ID=14245148
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP09934998A Expired - Fee Related JP3330872B2 (ja) | 1998-04-10 | 1998-04-10 | 追尾装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3330872B2 (ja) |
-
1998
- 1998-04-10 JP JP09934998A patent/JP3330872B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH11295417A (ja) | 1999-10-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3842746A2 (en) | Dead reckoning method and apparatus for vehicle, device and storage medium | |
| JP3179355B2 (ja) | 多目標追尾方法及び多目標追尾装置 | |
| CN112683269B (zh) | 一种附有运动加速度补偿的marg姿态计算方法 | |
| Chai et al. | An adaptive estimator for registration in augmented reality | |
| JP2007263637A (ja) | 測位装置及び測位方法及びプログラム | |
| JP3172689B2 (ja) | ストラップダウン慣性航法装置で使用するためのコーニング補償の方法、ならびに、そのための装置およびデジタルプロセッサ | |
| JP5219547B2 (ja) | 車載ナビゲーションシステム及びナビゲーション方法 | |
| JP3330872B2 (ja) | 追尾装置 | |
| JP2003329771A (ja) | 追尾装置および追尾処理方法 | |
| JP2009014596A (ja) | 目標追尾装置及び目標追尾方法 | |
| JP3333131B2 (ja) | 追尾装置 | |
| JP3097471B2 (ja) | 追尾装置 | |
| EP3134705B1 (en) | Initializing an inertial sensor using soft constraints and penalty functions | |
| CN114002650A (zh) | 一种多传感器空时偏差校准及机动目标跟踪方法 | |
| JP3432737B2 (ja) | 追尾装置 | |
| CN113483752A (zh) | 一种基于加速度匹配的航向快速初始化方法、电子设备、存储介质 | |
| JPH0429034B2 (ja) | ||
| JP2739054B2 (ja) | 目標運動解析方法 | |
| Liao et al. | Dynamic attitude estimation improvement for low-cost MEMS IMU by integrating low-cost GPS | |
| JPH05238493A (ja) | 人工衛星の制御装置 | |
| JPS59191608A (ja) | 処理スル−プツト速度の改善方法 | |
| JPS6010309A (ja) | ロボツトハンドの経路補間方法 | |
| JP3323415B2 (ja) | 目標追尾装置及びその目標追尾方法 | |
| JP2001083240A (ja) | 追尾装置 | |
| CN116810789A (zh) | 一种机器人防碰撞检测方法、设备及存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20070719 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (prs date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080719 Year of fee payment: 6 |
|
| LAPS | Cancellation because of no payment of annual fees |