知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-10-31
(45)【発行日】2024-11-11
(54)【発明の名称】目標追尾装置及び目標追尾方法
(51)【国際特許分類】
G01S 13/66 20060101AFI20241101BHJP
【ＦＩ】
G01S13/66
【請求項の数】
6
(21)【出願番号】P 2023573317
(86)(22)【出願日】2022-02-17
(86)【国際出願番号】 JP2022006289
(87)【国際公開番号】W WO2023157157
(87)【国際公開日】2023-08-24
【審査請求日】2023-11-27
【早期審査対象出願】
(73)【特許権者】
【識別番号】000006013
【氏名又は名称】三菱電機株式会社
(74)【代理人】
【識別番号】110003166
【氏名又は名称】弁理士法人山王内外特許事務所
(72)【発明者】
【氏名】亀田 洋志
(72)【発明者】
【氏名】白石 將
(72)【発明者】
【氏名】川南 遼平
(72)【発明者】
【氏名】系 正義
【審査官】小川 浩史
(56)【参考文献】
【文献】特開昭６１－１６９７８６（ＪＰ，Ａ）
【文献】特開２００５－２７４３００（ＪＰ，Ａ）
【文献】特開平９－１８９７６１（ＪＰ，Ａ）
【文献】特開昭６４－７３２７５（ＪＰ，Ａ）
【文献】米国特許第５３１３２１２（ＵＳ，Ａ）
【文献】特開２０１３－２５３９０９（ＪＰ，Ａ）
【文献】特開２０１２－２５１９０４（ＪＰ，Ａ）
【文献】特開２００９－１４５９６（ＪＰ，Ａ）
【文献】特開２００３－１６１７７８（ＪＰ，Ａ）
【文献】特開２００２－３２８１６２（ＪＰ，Ａ）
【文献】特開平１１－３０４９１５（ＪＰ，Ａ）
【文献】特開平９－２９７１７６（ＪＰ，Ａ）
【文献】特開平６－３４７５４２（ＪＰ，Ａ）
【文献】ZENG, Jianghui; GAO, Yongming; DING, Dan，“Positioning and Tracking Performance Analysis of Hypersonic Vehicle based on Aerodynamic Model”，2019 IEEE International Conference on Artificial Intelligence and Computer Applications (ICAICA)，2019年03月29日，pp. 312-318，DOI: 10.1109/ICAICA.2019.8873439
【文献】HUANG, Jingshuai; ZHANG, Hongbo; TANG, Guojian; BAO, Weimin，“Radar Tracking for Hypersonic Glide Vehicle Based on Aerodynamic Model”，2017 29th Chinese Control And Decision Conference (CCDC)，2017年05月28日，pp. 1080-1084，DOI: 10.1109/CCDC.2017.7978679
【文献】WATSON, G.A.; BLAIR, W.D.，“Interacting Acceleration Compensation Algorithm For Tracking Maneuvering Targets”，IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS，1995年07月，Vol. 31, No. 3，pp. 1152-1159，DOI: 10.1109/7.395225
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｓ １３／００－１３／９５
Ｇ０１Ｓ ７／００－７／４２
(57)【特許請求の範囲】
【請求項1】
閉ループを成す、平滑部と予測部と、を備え、前記平滑部は、追尾対象の観測位置を示す観測位置情報を取得し、かつ、前記追尾対象の観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、前記位置観測時点における前記追尾対象の状態の予測値を取得し、前記観測位置情報と前記追尾対象の状態の予測値とから、前記位置観測時点における前記追尾対象の状態を推定し、
前記観測位置情報と、前記位置観測時点における前記追尾対象の状態の予測値とから、前記追尾対象の運動モデルに含まれている前記追尾対象の機動方向の変化に係る加速度項から導かれる定数加速度ベクトルを補正し、
前記予測部は、前記平滑部により推定された状態から、前記位置観測時点よりも１サンプリング後の前記追尾対象の状態を予測し、前記追尾対象の状態の予測値を前記平滑部に出力する、
目標追尾装置。
【請求項2】
前記予測部は、前記平滑部による補正後の定数加速度ベクトルに基づく運動モデルを用いて、前記平滑部により推定された状態から、前記位置観測時点よりも１サンプリング後の前記追尾対象の状態を予測し、前記追尾対象の状態の予測値を前記平滑部に出力することを特徴とする請求項１記載の目標追尾装置。
【請求項3】
前記予測部が用いる複数の運動モデルは、それぞれ、前記追尾対象の機動方向の変化に係る加速度項のほか、重力及び空気力のそれぞれに係る項を含むものである、
請求項１に記載の目標追尾装置。
【請求項4】
前記予測部は、前記平滑部とともに前記閉ループを成す、複数の予測処理部と予測値統合部とを備え、複数の前記予測処理部のそれぞれは、互いに異なる複数の運動モデルのうち対応する１つを用いて、前記平滑部により推定された状態から、前記追尾対象の状態を予測し、前記追尾対象の状態のそれぞれに対して予測した値を前記予測値統合部に出力し、
前記予測値統合部は、複数の前記予測処理部から出力された予測した値を統合し、統合した値を前記平滑部に出力する、
請求項１に記載の目標追尾装置。
【請求項5】
前記観測位置情報と、前記位置観測時点における前記追尾対象の状態の予測値とを取得し、前記観測位置情報が示す前記追尾対象の観測位置と、前記追尾対象の状態の予測値に含まれている前記追尾対象の予測位置との差分が閾値以下であれば、前記観測位置情報を前記平滑部に出力する追尾ゲート部を備えたことを特徴とする請求項１記載の目標追尾装置。
【請求項6】
閉ループを成す、平滑部と予測部と、を備える目標追尾装置の目標追尾方法であって、前記平滑部が、
追尾対象の観測位置を示す観測位置情報を取得し、かつ、前記追尾対象の観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、前記位置観測時点における前記追尾対象の状態の予測値を取得し、前記観測位置情報と前記追尾対象の状態の予測値とから、前記位置観測時点における前記追尾対象の状態を推定し、
前記観測位置情報と、前記位置観測時点における前記追尾対象の状態の予測値とから、前記追尾対象の運動モデルに含まれている前記追尾対象の機動方向の変化に係る加速度項から導かれる定数加速度ベクトルを補正し、
前記予測部が、
前記平滑部により推定された状態から、前記位置観測時点よりも１サンプリング後の前記追尾対象の状態を予測し、前記追尾対象の状態の予測値を前記平滑部に出力する、
目標追尾方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、目標追尾装置及び目標追尾方法に関するものである。
【背景技術】
【0002】
追尾対象の運動モデルを用いて、追尾対象の状態を予測する目標追尾装置がある（例えば、非特許文献１を参照）。追尾対象の状態としては、例えば、追尾対象の位置、追尾対象の速度及び追尾対象の空力係数がある。
当該目標追尾装置は、平滑部及び予測部を備えている。平滑部は、追尾対象の観測位置を示す観測位置情報を取得し、かつ、予測部から予測値を取得する。予測値は、観測位置が観測された時点である位置観測時点よりも１サンプリング前に、予測部によって予測された、位置観測時点における追尾対象の状態の予測結果である。予測部は、追尾対象の運動モデル（式（８）を参照）を用いて、平滑部により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測し、追尾対象の状態の予測値を平滑部に出力する。
当該目標追尾装置は、平滑部及び予測部を備えている。平滑部は、追尾対象の観測位置を示す観測位置情報を取得し、かつ、予測部から予測値を取得する。予測値は、観測位置が観測された時点である位置観測時点よりも１サンプリング前に、予測部によって予測された、位置観測時点における追尾対象の状態の予測結果である。予測部は、追尾対象の運動モデル（式（８）を参照）を用いて、平滑部により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測し、追尾対象の状態の予測値を平滑部に出力する。
【先行技術文献】
【非特許文献】
【0003】
【文献】Ｊｉｎｇｓｈｕａｉ Ｈｕａｎｇ， Ｈｏｎｇｂｏ Ｚｈａｎｇ， Ｇｕｏｊｉａｎ Ｔａｎｇ， Ｗｅｉｍｉｎ Ｂａｏ，“Ｒａｄａｒ Ｔｒａｃｋｉｎｇ ｆｏｒ Ｈｙｐｅｒｓｏｎｉｃ Ｇｌｉｄｅ Ｖｅｈｉｃｌｅ Ｂａｓｅｄ ｏｎ Ａｅｒｏｄｙｎａｍｉｃ Ｍｏｄｅｌ”，２０１７ ２９ｔｈ Ｃｈｉｎｅｓｅ Ｃｏｎｔｒｏｌ Ａｎｄ Ｄｅｃｉｓｉｏｎ Ｃｏｎｆｅｒｅｎｃｅ （ＣＣＤＣ），２８－３０ Ｍａｙ ２０１７，ｐ．１０８０－１０８４
【発明の概要】
【発明が解決しようとする課題】
【0004】
追尾対象の将来の状態は、追尾対象の機動方向が変化することでも変化する。非特許文献１に開示されている目標追尾装置の予測部が用いる追尾対象の運動モデルでは、追尾対象の機動方向の変化が考慮されていないという課題があった。このため、当該目標追尾装置では、例えば、追尾対象の姿勢角の変化に伴って、追尾対象の機動方向が変化したときに、予測部による追尾対象の状態の予測精度が劣化してしまうことがある。
【0005】
本開示は、上記のような課題を解決するためになされたもので、追尾対象の機動方向を考慮した追尾対象の状態予測を行うことができる目標追尾装置及び目標追尾方法を得ることを目的とする。
【課題を解決するための手段】
【0006】
本開示に係る目標追尾装置は、閉ループを成す、平滑部と予測部と、を備え、平滑部は、追尾対象の観測位置を示す観測位置情報を取得し、かつ、追尾対象の観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、位置観測時点における追尾対象の状態の予測値を取得し、観測位置情報と追尾対象の状態の予測値とから、位置観測時点における追尾対象の状態を推定し、観測位置情報と、位置観測時点における追尾対象の状態の予測値とから、追尾対象の運動モデルに含まれている追尾対象の機動方向の変化に係る加速度項から導かれる定数加速度ベクトルを補正し、予測部は、平滑部により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測し、追尾対象の状態の予測値を平滑部に出力する、というものである。
【発明の効果】
【0007】
本開示によれば、追尾対象の機動方向を考慮した追尾対象の状態予測を行うことができる。
【図面の簡単な説明】
【0008】
【図1】実施の形態１に係る目標追尾装置を示す構成図である。
【図2】実施の形態１に係る目標追尾装置のハードウェアを示すハードウェア構成図である。
【図3】目標追尾装置が、ソフトウェア又はファームウェア等によって実現される場合のコンピュータのハードウェア構成図である。
【図4】定数加速度ベクトルを示す説明図である。
【図5】目標追尾装置の処理手順である目標追尾方法を示すフローチャートである。
【図6】実施の形態２に係る目標追尾装置を示す構成図である。
【図7】実施の形態２に係る目標追尾装置のハードウェアを示すハードウェア構成図である。
【発明を実施するための形態】
【0009】
以下、本開示をより詳細に説明するために、本開示を実施するための形態について、添付の図面に従って説明する。
【0010】
実施の形態１．
図１は、実施の形態１に係る目標追尾装置を示す構成図である。
図２は、実施の形態１に係る目標追尾装置のハードウェアを示すハードウェア構成図である。
図１に示す目標追尾装置は、追尾ゲート部１、平滑部２及び予測部３を備えている。
図１は、実施の形態１に係る目標追尾装置を示す構成図である。
図２は、実施の形態１に係る目標追尾装置のハードウェアを示すハードウェア構成図である。
図１に示す目標追尾装置は、追尾ゲート部１、平滑部２及び予測部３を備えている。
【0011】
追尾ゲート部１は、例えば、図２に示す追尾ゲート回路１１によって実現される。
追尾ゲート部１は、例えば、図示せぬ目標観測装置から、追尾対象の観測位置を示す観測位置情報を取得し、予測部３から、追尾対象の状態の予測値を取得する。予測値は、観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、位置観測時点における追尾対象の状態の予測結果を示すものである。
追尾ゲート部１は、観測位置情報が示す追尾対象の観測位置と、予測値に含まれている追尾対象の予測位置との差分を算出する。
追尾ゲート部１は、観測位置と予測位置との差分が閾値以下であれば、観測位置情報を平滑部２に出力し、差分が閾値よりも大きければ、観測位置情報を破棄する。閾値は、追尾ゲート部１の内部メモリに格納されていてもよいし、図１に示す目標追尾装置の外部から与えられるものであってもよい。
追尾ゲート部１は、例えば、図示せぬ目標観測装置から、追尾対象の観測位置を示す観測位置情報を取得し、予測部３から、追尾対象の状態の予測値を取得する。予測値は、観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、位置観測時点における追尾対象の状態の予測結果を示すものである。
追尾ゲート部１は、観測位置情報が示す追尾対象の観測位置と、予測値に含まれている追尾対象の予測位置との差分を算出する。
追尾ゲート部１は、観測位置と予測位置との差分が閾値以下であれば、観測位置情報を平滑部２に出力し、差分が閾値よりも大きければ、観測位置情報を破棄する。閾値は、追尾ゲート部１の内部メモリに格納されていてもよいし、図１に示す目標追尾装置の外部から与えられるものであってもよい。
【0012】
平滑部２は、例えば、図２に示す平滑回路１２によって実現される。
平滑部２は、追尾ゲート部１から、観測位置情報を取得し、予測部３から、予測値を取得する。
平滑部２は、観測位置情報と予測値とから、位置観測時点における追尾対象の状態を推定する。
平滑部２は、追尾対象の状態の推定値を外部に出力し、かつ、追尾対象の状態の推定値を予測部３に出力する。
平滑部２は、追尾ゲート部１から、観測位置情報を取得し、予測部３から、予測値を取得する。
平滑部２は、観測位置情報と予測値とから、位置観測時点における追尾対象の状態を推定する。
平滑部２は、追尾対象の状態の推定値を外部に出力し、かつ、追尾対象の状態の推定値を予測部３に出力する。
【0013】
予測部３は、例えば、図２に示す予測回路１３によって実現される。
予測部３は、Ｎ個の予測処理部４－１～４－Ｎと、予測値統合部５とを備えている。Ｎは、１以上の整数である。
予測部３は、平滑部２から、追尾対象の状態の推定値を取得する。
予測部３は、追尾対象の運動モデルとして、追尾対象の機動方向の変化に係る加速度項を含む運動モデルを用いて、平滑部２により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測する。
予測部３は、追尾対象の状態の予測値を追尾ゲート部１及び平滑部２のそれぞれに出力する。
予測部３は、Ｎ個の予測処理部４－１～４－Ｎと、予測値統合部５とを備えている。Ｎは、１以上の整数である。
予測部３は、平滑部２から、追尾対象の状態の推定値を取得する。
予測部３は、追尾対象の運動モデルとして、追尾対象の機動方向の変化に係る加速度項を含む運動モデルを用いて、平滑部２により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測する。
予測部３は、追尾対象の状態の予測値を追尾ゲート部１及び平滑部２のそれぞれに出力する。
【0014】
予測処理部４－ｎ（ｎ＝１，・・・，Ｎ）は、追尾対象の運動モデルとして、重力及び空気力のそれぞれに係る項と、追尾対象の機動方向の変化に係る加速度項とを含む運動モデルを備えている。予測処理部４－１～４－Ｎのそれぞれが備えている運動モデルは、互いに異なる運動モデルである。
図１に示す目標追尾装置では、追尾対象の運動モデルが、重力及び空気力のそれぞれに係る項と、追尾対象の機動方向の変化に係る加速度項とを含んでいる。しかし、追尾対象の運動モデルは、追尾対象の機動方向の変化に係る加速度項を含んでいればよく、重力及び空気力のそれぞれに係る項を含んでいなくてもよい。
予測処理部４－ｎは、運動モデルを用いて、平滑部２により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測し、追尾対象の状態の予測値を予測値統合部５に出力する。
図１に示す目標追尾装置では、予測処理部４－ｎが、運動モデルを備えている。しかし、これは一例に過ぎず、運動モデルが目標追尾装置の外部から与えられるものであってもよい。
図１に示す目標追尾装置では、追尾対象の運動モデルが、重力及び空気力のそれぞれに係る項と、追尾対象の機動方向の変化に係る加速度項とを含んでいる。しかし、追尾対象の運動モデルは、追尾対象の機動方向の変化に係る加速度項を含んでいればよく、重力及び空気力のそれぞれに係る項を含んでいなくてもよい。
予測処理部４－ｎは、運動モデルを用いて、平滑部２により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測し、追尾対象の状態の予測値を予測値統合部５に出力する。
図１に示す目標追尾装置では、予測処理部４－ｎが、運動モデルを備えている。しかし、これは一例に過ぎず、運動モデルが目標追尾装置の外部から与えられるものであってもよい。
【0015】
予測値統合部５は、予測処理部４－１～４－Ｎのそれぞれから予測値を取得する。
予測値統合部５は、Ｎ個の予測値を統合し、統合後の予測値を追尾ゲート部１及び平滑部２のそれぞれに出力する。
予測値統合部５は、Ｎ個の予測値を統合し、統合後の予測値を追尾ゲート部１及び平滑部２のそれぞれに出力する。
【0016】
図１では、目標追尾装置の構成要素である追尾ゲート部１、平滑部２及び予測部３のそれぞれが、図２に示すような専用のハードウェアによって実現されるものを想定している。即ち、目標追尾装置が、追尾ゲート回路１１、平滑回路１２及び予測回路１３によって実現されるものを想定している。
追尾ゲート回路１１、平滑回路１２及び予測回路１３のそれぞれは、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、又は、これらを組み合わせたものが該当する。
追尾ゲート回路１１、平滑回路１２及び予測回路１３のそれぞれは、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、又は、これらを組み合わせたものが該当する。
【0017】
目標追尾装置の構成要素は、専用のハードウェアによって実現されるものに限るものではなく、目標追尾装置が、ソフトウェア、ファームウェア、又は、ソフトウェアとファームウェアとの組み合わせによって実現されるものであってもよい。
ソフトウェア又はファームウェアは、プログラムとして、コンピュータのメモリに格納される。コンピュータは、プログラムを実行するハードウェアを意味し、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、あるいは、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）が該当する。
ソフトウェア又はファームウェアは、プログラムとして、コンピュータのメモリに格納される。コンピュータは、プログラムを実行するハードウェアを意味し、例えば、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサ、あるいは、ＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）が該当する。
【0018】
図３は、目標追尾装置が、ソフトウェア又はファームウェア等によって実現される場合のコンピュータのハードウェア構成図である。
目標追尾装置が、ソフトウェア又はファームウェア等によって実現される場合、追尾ゲート部１、平滑部２及び予測部３におけるそれぞれの処理手順をコンピュータに実行させるためのプログラムがメモリ３１に格納される。そして、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行する。
目標追尾装置が、ソフトウェア又はファームウェア等によって実現される場合、追尾ゲート部１、平滑部２及び予測部３におけるそれぞれの処理手順をコンピュータに実行させるためのプログラムがメモリ３１に格納される。そして、コンピュータのプロセッサ３２がメモリ３１に格納されているプログラムを実行する。
【0019】
また、図２では、目標追尾装置の構成要素のそれぞれが専用のハードウェアによって実現される例を示し、図３では、目標追尾装置がソフトウェア又はファームウェア等によって実現される例を示している。しかし、これは一例に過ぎず、目標追尾装置における一部の構成要素が専用のハードウェアによって実現され、残りの構成要素がソフトウェア又はファームウェア等によって実現されるものであってもよい。
【0020】
最初に、図１に示す目標追尾装置による追尾対象の状態推定原理を説明する。
追尾対象の状態を示す状態ベクトルｘｋは、例えば、以下の式（１）に示すように、追尾対象の位置ベクトルｘｋ，ｙｋ，ｚｋ、追尾対象の速度ベクトルｘドットｋ，ｙドットｋ，ｚドットｋ及び空力係数α，β，γによって表される。ｋは、１以上の整数である。
状態ベクトルｘｋと位置ベクトルｘｋとは、同じｘｋで表しているが、位置ベクトルｘｋは、スカラーである。位置ベクトルｙｋ，ｚｋについてもスカラーである。位置ベクトルｘｋ，ｙｋ，ｚｋのそれぞれは、例えば、北基準直交座標系における追尾対象の３次元位置を表している。
追尾対象の状態を示す状態ベクトルｘｋは、例えば、以下の式（１）に示すように、追尾対象の位置ベクトルｘｋ，ｙｋ，ｚｋ、追尾対象の速度ベクトルｘドットｋ，ｙドットｋ，ｚドットｋ及び空力係数α，β，γによって表される。ｋは、１以上の整数である。
状態ベクトルｘｋと位置ベクトルｘｋとは、同じｘｋで表しているが、位置ベクトルｘｋは、スカラーである。位置ベクトルｙｋ，ｚｋについてもスカラーである。位置ベクトルｘｋ，ｙｋ，ｚｋのそれぞれは、例えば、北基準直交座標系における追尾対象の３次元位置を表している。
【0021】
明細書の文章中では、電子出願の関係上、文字（ｘｋ，ｙｋ，ｚｋ）の上に“・”の記号を付することができないため、速度ベクトルｘドットｋ，ｙドットｋ，ｚドットｋのように表記している。
αは、揚力を示す空力係数、βは、抗力を示す空力係数、γは、空気力を示す空力係数である。
αは、揚力を示す空力係数、βは、抗力を示す空力係数、γは、空気力を示す空力係数である。
【0022】
【0023】
図示せぬ目標観測装置から追尾ゲート部１に与えられる観測位置情報を示す観測ベクトルｚｋは、以下の式（２）に示すように、追尾対象の３次元の観測位置を示すベクトルであり、観測ベクトルｚｋの要素であるｘｋ，ｙｋ，ｚｋのそれぞれは、スカラーである。
【0024】
【0025】
予測処理部４－ｎ（ｎ＝１，・・・，Ｎ）に用いられる運動モデルは、以下の式（３）に示すように、式（１）に示す状態ベクトルｘｋについての１階の微分方程式で表すことができる。
【0026】
【0027】
式（３）において、ｗｋは、駆動雑音ベクトルであり、以下の式（４）及び式（５）に示す性質を有している。
【0028】
【0029】
式（３）において、ｆ（ｘｋ）は、以下の式（６）に示すように、空力係数α，β，γを用いて表される。
式（５）において、Ｑｋは、駆動雑音誤差共分散行列である。式（５ａ）において、０m×ｎは、ｍ行ｎ列の零行列、Ｑｋ ｐｖは、位置及び速度に関する駆動雑音行列パラメータ、Ｑα，Ｑβ，Ｑγのそれぞれは、空力係数に関する駆動雑音行列パラメータ（スカラー値）である。ｄｉａｇ［Ａ，Ｂ，Ｃ］は、Ａ，Ｂ，Ｃを要素とする対角行列である。この例では、３行３列である。
式（５）において、Ｑｋは、駆動雑音誤差共分散行列である。式（５ａ）において、０m×ｎは、ｍ行ｎ列の零行列、Ｑｋ ｐｖは、位置及び速度に関する駆動雑音行列パラメータ、Ｑα，Ｑβ，Ｑγのそれぞれは、空力係数に関する駆動雑音行列パラメータ（スカラー値）である。ｄｉａｇ［Ａ，Ｂ，Ｃ］は、Ａ，Ｂ，Ｃを要素とする対角行列である。この例では、３行３列である。
【0030】
式（６）において、μは、万有引力定数、ρは、空気密度、ｒは、距離、Ｖは、速度である。
【0031】
式（３）に示す運動モデルは、以下の式（７）に示すように、離散系で表現することができる。
【0032】
【0033】
予測処理部４－ｎ（ｎ＝１，・・・，Ｎ）に用いられる運動モデルは、以下の式（９）に示すように、駆動雑音ベクトルｗｋ及び定数加速度ベクトルの変換行列Γ’ｋ－１のそれぞれによって拡張されている。
【0034】
Γ’ｋ－１は、サンプリング時刻ｔｋ－１における定数加速度ベクトルの変換行列であり、以下の式（１１）のように表される。
【0035】
式（１１）において、Ｆｋは、定数加速度ベクトルｕｋを定義した座標系から、状態ベクトルｘｋを定義した座標系への座標変換行列である。Ｉm×ｎは、ｍ行ｎ列の単位行列である。
【0036】
図４は、定数加速度ベクトルを示す説明図である。
図４において、Ｏは、目標観測装置を原点とした座標Ｏ－ｘｙｚの原点、Ｘは、東方向をｘ軸の正とした座標Ｏ－ｘｙｚのｘ軸、Ｙは、北方向をｙ軸の正とした座標Ｏ－ｘｙｚのｙ軸、Ｚは、上方向をｚ軸の正とした座標Ｏ－ｘｙｚのｚ軸である。
α１は、ｙ軸正方向の定数加速度ベクトル、α２は、ｙ軸負方向の定数加速度ベクトルである。
α３は、ｘ軸正方向の定数加速度ベクトル、α４は、ｘ軸負方向の定数加速度ベクトルである。
α５は、ｚ軸正方向の定数加速度ベクトル、α６は、ｚ軸負方向の定数加速度ベクトルである。
定数加速度ベクトルα１～α６の他に、定数加速度ベクトルα７＝０を定義すると、運動モデルの数は、７個となる。定数加速度の１軸には、北基準直交座標系の北方向、推定速度ベクトル、又は、ＬＯＳ（Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ）等が考えられる。ＬＯＳは、目標観測装置から追尾対象を見た視線方向のベクトルである。
図４において、Ｏは、目標観測装置を原点とした座標Ｏ－ｘｙｚの原点、Ｘは、東方向をｘ軸の正とした座標Ｏ－ｘｙｚのｘ軸、Ｙは、北方向をｙ軸の正とした座標Ｏ－ｘｙｚのｙ軸、Ｚは、上方向をｚ軸の正とした座標Ｏ－ｘｙｚのｚ軸である。
α１は、ｙ軸正方向の定数加速度ベクトル、α２は、ｙ軸負方向の定数加速度ベクトルである。
α３は、ｘ軸正方向の定数加速度ベクトル、α４は、ｘ軸負方向の定数加速度ベクトルである。
α５は、ｚ軸正方向の定数加速度ベクトル、α６は、ｚ軸負方向の定数加速度ベクトルである。
定数加速度ベクトルα１～α６の他に、定数加速度ベクトルα７＝０を定義すると、運動モデルの数は、７個となる。定数加速度の１軸には、北基準直交座標系の北方向、推定速度ベクトル、又は、ＬＯＳ（Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ）等が考えられる。ＬＯＳは、目標観測装置から追尾対象を見た視線方向のベクトルである。
【0037】
次に、サンプリング時刻ｔｋにおいて、以下の式（１２）が真である場合の運動モデルを以下の式（１３）のように定義する。
【0038】
【0039】
ここでは、運動モデルΨｋ，ａの推移がマルコフ性であると仮定する。即ち、運動モデルΨｋ，ａは、サンプリング時刻ｔｋ－１の運動モデルΨｋ－１，ａによって決まり、サンプリング時刻ｔｋ－２までの運動モデルΨｋ－２，ａ等には依存しないものとする。
例えば、最小サンプリング間隔がｔｓ（＝ｔｋ－ｔｋ－１）である場合の運動モデル間推移確率ｐｋ，ａｂは、以下の式（１４）に示すように表される。
Ｎ×Ｎ行列において、ａ行ｂ列の要素をｐａｂとしたものを、運動モデル間推移確率行列Πで表し、サンプリング間隔をｔｋ－ｔｋ－１＝ｍｔｓ（ｍ＝１，２，・・・）で表す場合、サンプリング時刻ｔｋでの運動モデル間推移確率行列Πｋは、以下の式（１５）のように表される。運動モデル間推移確率行列Πｋのａ行ｂ列の要素が運動モデル間推移確率ｐｋ，ａｂである。
例えば、最小サンプリング間隔がｔｓ（＝ｔｋ－ｔｋ－１）である場合の運動モデル間推移確率ｐｋ，ａｂは、以下の式（１４）に示すように表される。
Ｎ×Ｎ行列において、ａ行ｂ列の要素をｐａｂとしたものを、運動モデル間推移確率行列Πで表し、サンプリング間隔をｔｋ－ｔｋ－１＝ｍｔｓ（ｍ＝１，２，・・・）で表す場合、サンプリング時刻ｔｋでの運動モデル間推移確率行列Πｋは、以下の式（１５）のように表される。運動モデル間推移確率行列Πｋのａ行ｂ列の要素が運動モデル間推移確率ｐｋ，ａｂである。
【0040】
【0041】
サンプリング時刻ｔｋまでの観測情報Ｚｋに基づく、サンプリング時刻ｔｋでの運動モデルΨｋ，ａの信頼度μｋ，ａ（＋）は、以下の式（１６）に示すように、条件付き確率密度関数Ｐ［Ψｋ，ａ｜Ｚｋ］によって定義すると、以下の式（１７）のように表現できる。
【0042】
【0043】
サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１に基づく、サンプリング時刻ｔｋでの運動モデルΨｋ，ａの事前信頼度μｋ，ａ（－）は、以下の式（１９）に示すように、条件付き確率密度関数Ｐ［Ψｋ，ａ｜Ｚｋ－１］によって定義すると、以下の式（２０）のように表現できる。
【0044】
【0045】
サンプリング時刻ｔｋまでの観測情報Ｚｋに基づく、定数加速度ベクトルの推定値ｕｋ－１（＋）は、以下の式（２１）のように定義すると、以下の式（２２）のように表現できる。以下、定数加速度ベクトルの推定値ｕｋ－１（＋）を推定加速度ベクトルという。
【0046】
【0047】
サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１に基づく、定数加速度ベクトルの推定値ｕｋ－１（－）は、以下の式（２３）のように定義すると、以下の式（２４）のように表現できる。以下、定数加速度ベクトルの推定値ｕｋ－１（－）を予測加速度ベクトルという。
【0048】
【0049】
式（２）に示す観測ベクトルｚｋに関する観測モデルＺｋは、以下の式（２５）のように表される。
【0050】
ｖｋは、サンプリング時刻ｔｋでの観測ベクトルｚｋに対応する観測雑音ベクトルであり、観測雑音ベクトルは、以下の式（２７）に示すように、平均が０の３次元正規分布白色雑音である。
【0051】
【0052】
サンプリング時刻ｔ１からサンプリング時刻ｔｋまでの観測ベクトルの蓄積は、以下の式（２９）のように表される。
【0053】
【0054】
カルマンフィルタの理論によれば、式（２５）に示す観測モデルに従ってサンプリング時刻ｔｋで観測値が得られた場合の状態ベクトルｘｋの推定値Ｘｋ（＋）は、以下の式（３０）～（３８）によって計算される。
【0055】
【0056】
【0057】
Ｘハットｋ，ａ（－）は、サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１と運動モデルΨｋ，ａとに基づくＸｋの条件付き平均値である。当該平均値は、サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１と運動モデルΨｋ，ａとに基づいてサンプリング時刻ｔｋの真値を推定した予測ベクトルに相当する。
Ｘハットｋ，ａ（＋）は、サンプリング時刻ｔｋまでの観測情報Ｚｋと運動モデルΨｋ，ａとに基づくＸｋの条件付き平均値である。当該平均値は、サンプリング時刻ｔｋまでの観測情報Ｚｋと運動モデルΨｋ，ａとに基づいてサンプリング時刻ｔｋの真値を推定した平滑ベクトルに相当する。
Ｘハットｋ（－）は、サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１に基づくＸｋの条件付き平均値である。当該平均値は、サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１に基づいてサンプリング時刻ｔｋの真値を推定した予測ベクトルに相当する。
Ｘハットｋ（＋）は、サンプリング時刻ｔｋまでの観測情報Ｚｋに基づくＸｋの条件付き平均値である。当該平均値は、サンプリング時刻ｔｋまでの観測情報Ｚｋに基づいてサンプリング時刻ｔｋの真値を推定した平滑ベクトルに相当する。
明細書の文書中では、電子出願の関係上、文字（Ｘｋ，ａ（－），Ｘｋ，ａ（＋），Ｘｋ（－），Ｘｋ（＋））の上に“＾”の記号を付することができないため、Ｘハットｋ，ａ（－），Ｘハットｋ，ａ（＋），Ｘハットｋ（－），Ｘハットｋ（＋）のように表記している。
Ｘハットｋ，ａ（＋）は、サンプリング時刻ｔｋまでの観測情報Ｚｋと運動モデルΨｋ，ａとに基づくＸｋの条件付き平均値である。当該平均値は、サンプリング時刻ｔｋまでの観測情報Ｚｋと運動モデルΨｋ，ａとに基づいてサンプリング時刻ｔｋの真値を推定した平滑ベクトルに相当する。
Ｘハットｋ（－）は、サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１に基づくＸｋの条件付き平均値である。当該平均値は、サンプリング時刻ｔｋ－１までの観測情報Ｚｋ－１に基づいてサンプリング時刻ｔｋの真値を推定した予測ベクトルに相当する。
Ｘハットｋ（＋）は、サンプリング時刻ｔｋまでの観測情報Ｚｋに基づくＸｋの条件付き平均値である。当該平均値は、サンプリング時刻ｔｋまでの観測情報Ｚｋに基づいてサンプリング時刻ｔｋの真値を推定した平滑ベクトルに相当する。
明細書の文書中では、電子出願の関係上、文字（Ｘｋ，ａ（－），Ｘｋ，ａ（＋），Ｘｋ（－），Ｘｋ（＋））の上に“＾”の記号を付することができないため、Ｘハットｋ，ａ（－），Ｘハットｋ，ａ（＋），Ｘハットｋ（－），Ｘハットｋ（＋）のように表記している。
【0058】
Ｐｋ，ａ（－）は、Ｘハットｋ，ａ（－）の誤差共分散行列を示す運動モデル毎の予測誤差共分散行列である。
Ｐｋ，ａ（＋）は、Ｘハットｋ，ａ（＋）の誤差共分散行列を示す運動モデル毎の平滑誤差共分散行列である。
Ｐｋ（－）は、Ｘハットｋ（－）の誤差共分散行列を示すＮ個の運動モデルによる予測誤差共分散行列である。
Ｐｋ（＋）は、Ｘハットｋ（＋）の誤差共分散行列を示すＮ個の運動モデルによる平滑誤差共分散行列である。
式（３４）に示すＫｋは、サンプリング時刻ｔｋにおけるゲイン行列である。また、カルマンフィルタを適用する場合と同様に、初期値Ｘ０（＋）及び初期値Ｐ０（＋）のそれぞれは、別途定まっているものとする。
Ｐｋ，ａ（－）は、式（３１）に示すように、運動モデルΨｋ，ａに依らない値であるため、式（３４）に示すＫｋ及び式（３６）に示すＰｋ，ａ（＋）のそれぞれも、運動モデルΨｋ，ａに依らない値である。
Ｐｋ，ａ（＋）は、Ｘハットｋ，ａ（＋）の誤差共分散行列を示す運動モデル毎の平滑誤差共分散行列である。
Ｐｋ（－）は、Ｘハットｋ（－）の誤差共分散行列を示すＮ個の運動モデルによる予測誤差共分散行列である。
Ｐｋ（＋）は、Ｘハットｋ（＋）の誤差共分散行列を示すＮ個の運動モデルによる平滑誤差共分散行列である。
式（３４）に示すＫｋは、サンプリング時刻ｔｋにおけるゲイン行列である。また、カルマンフィルタを適用する場合と同様に、初期値Ｘ０（＋）及び初期値Ｐ０（＋）のそれぞれは、別途定まっているものとする。
Ｐｋ，ａ（－）は、式（３１）に示すように、運動モデルΨｋ，ａに依らない値であるため、式（３４）に示すＫｋ及び式（３６）に示すＰｋ，ａ（＋）のそれぞれも、運動モデルΨｋ，ａに依らない値である。
【0059】
次に、図１に示す目標追尾装置の動作について説明する。
図５は、目標追尾装置の処理手順である目標追尾方法を示すフローチャートである。
予測処理部４－ｎ（ｎ＝１，・・・，Ｎ）は、Ｎ個の定数加速度ベクトルα１～αＮのうち、いずれかの定数加速度ベクトルαｎを有する、式（１３）に示すような運動モデルΨｋ，ａ（ａ＝１，・・・，Ｎ）を備えている。
図５は、目標追尾装置の処理手順である目標追尾方法を示すフローチャートである。
予測処理部４－ｎ（ｎ＝１，・・・，Ｎ）は、Ｎ個の定数加速度ベクトルα１～αＮのうち、いずれかの定数加速度ベクトルαｎを有する、式（１３）に示すような運動モデルΨｋ，ａ（ａ＝１，・・・，Ｎ）を備えている。
【0060】
予測処理部４－ｎは、平滑部２から、位置観測時点における追尾対象の状態の推定値を取得する。
予測処理部４－ｎは、運動モデルΨｋ，ａを用いて、追尾対象の状態の推定値から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測する（図５のステップＳＴ１）。
予測処理部４－ｎは、追尾対象の状態の予測値を予測値統合部５に出力する。
予測処理部４－ｎは、運動モデルΨｋ，ａを用いて、追尾対象の状態の推定値から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測する（図５のステップＳＴ１）。
予測処理部４－ｎは、追尾対象の状態の予測値を予測値統合部５に出力する。
【0061】
以下、予測処理部４－ｎによる追尾対象の状態予測処理を具体的に説明する。
予測処理部４－ｎは、平滑部２から、追尾対象の状態の推定値として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）と、平滑誤差共分散行列Ｐｋ（＋）とを取得する。
予測処理部４－ｎは、式（３０）に示すｘｋ－１（＋）として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）を式（３０）に代入することで、追尾対象の状態の予測値として、予測ベクトルＸハットｋ，ａ（－）を算出する。
また、予測処理部４－ｎは、式（３１）に示すＰｋ－１（＋）として、平滑誤差共分散行列Ｐｋ（＋）を式（３１）に代入することで、予測誤差共分散行列Ｐｋ，ａ（－）を算出する。
予測処理部４－ｎは、予測ベクトルＸハットｋ，ａ（－）及び予測誤差共分散行列Ｐｋ，ａ（－）のそれぞれを予測値統合部５に出力する。
予測処理部４－ｎは、平滑部２から、追尾対象の状態の推定値として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）と、平滑誤差共分散行列Ｐｋ（＋）とを取得する。
予測処理部４－ｎは、式（３０）に示すｘｋ－１（＋）として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）を式（３０）に代入することで、追尾対象の状態の予測値として、予測ベクトルＸハットｋ，ａ（－）を算出する。
また、予測処理部４－ｎは、式（３１）に示すＰｋ－１（＋）として、平滑誤差共分散行列Ｐｋ（＋）を式（３１）に代入することで、予測誤差共分散行列Ｐｋ，ａ（－）を算出する。
予測処理部４－ｎは、予測ベクトルＸハットｋ，ａ（－）及び予測誤差共分散行列Ｐｋ，ａ（－）のそれぞれを予測値統合部５に出力する。
【0062】
予測値統合部５は、予測処理部４－１～４－Ｎから、追尾対象の状態の予測値として、Ｎ個の予測ベクトルＸハットｋ，ａ（－）を取得し、Ｎ個の予測ベクトルＸハットｋ，ａ（－）を統合する（図５のステップＳＴ２）。
統合後の予測値である予測ベクトルＸハットｋ（－）は、式（３２）に示すｘｋ－１（＋）として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）を式（３２）に代入することで算出されるものに相当する。
予測値統合部５は、予測ベクトルＸハットｋ，ａ（－）を平滑部２に出力し、予測ベクトルＸハットｋ（－）を追尾ゲート部１及び平滑部２のそれぞれに出力する。
統合後の予測値である予測ベクトルＸハットｋ（－）は、式（３２）に示すｘｋ－１（＋）として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）を式（３２）に代入することで算出されるものに相当する。
予測値統合部５は、予測ベクトルＸハットｋ，ａ（－）を平滑部２に出力し、予測ベクトルＸハットｋ（－）を追尾ゲート部１及び平滑部２のそれぞれに出力する。
【0063】
また、予測値統合部５は、予測処理部４－１～４－Ｎから、Ｎ個の予測誤差共分散行列Ｐｋ，ａ（－）を取得し、Ｎ個の予測誤差共分散行列Ｐｋ，ａ（－）を統合する。
即ち、予測値統合部５は、予測誤差共分散行列Ｐｋ，ａ（－）を式（３３）に代入することで、統合後の予測誤差共分散行列Ｐｋ（－）を算出する。
予測値統合部５は、予測誤差共分散行列Ｐｋ（－）を平滑部２に出力する。
即ち、予測値統合部５は、予測誤差共分散行列Ｐｋ，ａ（－）を式（３３）に代入することで、統合後の予測誤差共分散行列Ｐｋ（－）を算出する。
予測値統合部５は、予測誤差共分散行列Ｐｋ（－）を平滑部２に出力する。
【0064】
追尾ゲート部１は、例えば、図示せぬ目標観測装置から、追尾対象の観測位置を示す観測位置情報として、観測ベクトルｚｋを取得する。
また、追尾ゲート部１は、予測値統合部５から、位置観測時点よりも１サンプリング前に予測された予測値として、予測ベクトルＸハットｋ－１（－）を取得する。
追尾ゲート部１は、観測ベクトルｚｋの要素である追尾対象の観測位置（ｘｋ，ｙｋ，ｚｋ）と、予測ベクトルＸハットｋ－１（－）の要素である追尾対象の予測位置との差分を算出する。
追尾ゲート部１は、観測位置と予測位置との差分が閾値以下であれば（図５のステップＳＴ３：ＹＥＳの場合）、観測ベクトルｚｋを平滑部２に出力する（図５のステップＳＴ４）。
追尾ゲート部１は、観測位置と予測位置との差分が閾値よりも大きければ（図５のステップＳＴ３：ＮＯの場合）、観測ベクトルｚｋを破棄する（図５のステップＳＴ５）。
また、追尾ゲート部１は、予測値統合部５から、位置観測時点よりも１サンプリング前に予測された予測値として、予測ベクトルＸハットｋ－１（－）を取得する。
追尾ゲート部１は、観測ベクトルｚｋの要素である追尾対象の観測位置（ｘｋ，ｙｋ，ｚｋ）と、予測ベクトルＸハットｋ－１（－）の要素である追尾対象の予測位置との差分を算出する。
追尾ゲート部１は、観測位置と予測位置との差分が閾値以下であれば（図５のステップＳＴ３：ＹＥＳの場合）、観測ベクトルｚｋを平滑部２に出力する（図５のステップＳＴ４）。
追尾ゲート部１は、観測位置と予測位置との差分が閾値よりも大きければ（図５のステップＳＴ３：ＮＯの場合）、観測ベクトルｚｋを破棄する（図５のステップＳＴ５）。
【0065】
平滑部２は、追尾ゲート部１から、観測ベクトルｚｋを取得し、予測値統合部５から、位置観測時点よりも１サンプリング前に予測された予測値として、予測ベクトルＸハットｋ－１（－）を取得する。
平滑部２は、観測ベクトルｚｋと、予測ベクトルＸハットｋ－１（－）とから、追尾対象の状態を推定する（図５のステップＳＴ６）。
平滑部２は、追尾対象の状態の推定値として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）と、平滑誤差共分散行列Ｐｋ（＋）とを外部に出力する。
また、平滑部２は、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）と、平滑誤差共分散行列Ｐｋ（＋）とを予測部３に出力する。
平滑部２は、観測ベクトルｚｋと、予測ベクトルＸハットｋ－１（－）とから、追尾対象の状態を推定する（図５のステップＳＴ６）。
平滑部２は、追尾対象の状態の推定値として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）と、平滑誤差共分散行列Ｐｋ（＋）とを外部に出力する。
また、平滑部２は、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）と、平滑誤差共分散行列Ｐｋ（＋）とを予測部３に出力する。
【0066】
以下、平滑部２による追尾対象の状態推定処理を具体的に説明する。
まず、平滑部２は、サンプリング時刻ｔｋまでの観測情報Ｚｋを式（１６）に代入することで、サンプリング時刻ｔｋでの運動モデルΨｋ，ａの信頼度μｋ，ａ（＋）を算出する。
平滑部２は、信頼度μｋ，ａ（＋）を式（２２）に代入することで、推定加速度ベクトルｕｋ－１（＋）を算出する。
次に、平滑部２は、予測値統合部５から、予測誤差共分散行列Ｐｋ，ａ（－）を取得する。
平滑部２は、予測誤差共分散行列Ｐｋ，ａ（－）及び観測雑音共分散行列Ｒｋのそれぞれを式（３４）に代入することで、ゲイン行列Ｋｋを算出する。
まず、平滑部２は、サンプリング時刻ｔｋまでの観測情報Ｚｋを式（１６）に代入することで、サンプリング時刻ｔｋでの運動モデルΨｋ，ａの信頼度μｋ，ａ（＋）を算出する。
平滑部２は、信頼度μｋ，ａ（＋）を式（２２）に代入することで、推定加速度ベクトルｕｋ－１（＋）を算出する。
次に、平滑部２は、予測値統合部５から、予測誤差共分散行列Ｐｋ，ａ（－）を取得する。
平滑部２は、予測誤差共分散行列Ｐｋ，ａ（－）及び観測雑音共分散行列Ｒｋのそれぞれを式（３４）に代入することで、ゲイン行列Ｋｋを算出する。
【0067】
次に、平滑部２は、予測値統合部５から、予測ベクトルＸハットｋ，ａ（－）及び予測ベクトルＸハットｋ（－）のそれぞれを取得する。
平滑部２は、観測情報Ｚｋ及び予測ベクトルＸハットｋ，ａ（－）のそれぞれを式（３５）に代入することで、平滑ベクトルＸハットｋ，ａ（＋）を算出する。
また、平滑部２は、式（３７）に示すｘｋ－１（－）として、予測ベクトルＸハットｋ（－）を式（３７）に代入し、かつ、推定加速度ベクトルｕｋ－１（＋）を式（３７）に代入することで、平滑ベクトルＸハットｋ（＋）を算出する。
平滑部２は、観測情報Ｚｋ及び予測ベクトルＸハットｋ，ａ（－）のそれぞれを式（３５）に代入することで、平滑ベクトルＸハットｋ，ａ（＋）を算出する。
また、平滑部２は、式（３７）に示すｘｋ－１（－）として、予測ベクトルＸハットｋ（－）を式（３７）に代入し、かつ、推定加速度ベクトルｕｋ－１（＋）を式（３７）に代入することで、平滑ベクトルＸハットｋ（＋）を算出する。
【0068】
次に、平滑部２は、予測誤差共分散行列Ｐｋ，ａ（－）及びゲイン行列Ｋｋのそれぞれを式（３６）に代入することで、平滑誤差共分散行列Ｐｋ，ａ（＋）を算出する。
また、平滑部２は、平滑誤差共分散行列Ｐｋ，ａ（＋）及びゲイン行列Ｋｋのそれぞれを式（３８）に代入することで、平滑誤差共分散行列Ｐｋ（＋）を算出する。
また、平滑部２は、平滑誤差共分散行列Ｐｋ，ａ（＋）及びゲイン行列Ｋｋのそれぞれを式（３８）に代入することで、平滑誤差共分散行列Ｐｋ（＋）を算出する。
【0069】
以降、図１に示す目標追尾装置は、追尾対象の追尾を終了するまで、ステップＳＴ１～ＳＴ６の処理を繰り返し実施する。
【0070】
以上の実施の形態１では、追尾対象の観測位置を示す観測位置情報を取得し、かつ、観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、位置観測時点における追尾対象の状態の予測値を取得し、観測位置情報と予測値とから、位置観測時点における追尾対象の状態を推定する平滑部２と、追尾対象の運動モデルとして、追尾対象の機動方向の変化に係る加速度項を含む運動モデルを用いて、平滑部２により推定された状態から、位置観測時点よりも１サンプリング後の追尾対象の状態を予測し、追尾対象の状態の予測値を平滑部２に出力する予測部３とを備えるように、目標追尾装置を構成した。したがって、目標追尾装置は、追尾対象の機動方向を考慮した追尾対象の状態予測を行うことができる。
【0071】
実施の形態２．
実施の形態２では、平滑部６が、観測位置情報と、位置観測時点よりも１サンプリング前に予測された予測値とから、運動モデルに含まれている加速度項を補正する目標追尾装置について説明する。
実施の形態２では、平滑部６が、観測位置情報と、位置観測時点よりも１サンプリング前に予測された予測値とから、運動モデルに含まれている加速度項を補正する目標追尾装置について説明する。
【0072】
図６は、実施の形態２に係る目標追尾装置を示す構成図である。図６において、図１と同一符号は同一又は相当部分を示すので説明を省略する。
図７は、実施の形態２に係る目標追尾装置のハードウェアを示すハードウェア構成図である。図７において、図２と同一符号は同一又は相当部分を示すので説明を省略する。
図６に示す目標追尾装置は、追尾ゲート部１、平滑部６及び予測部７を備えている。
図７は、実施の形態２に係る目標追尾装置のハードウェアを示すハードウェア構成図である。図７において、図２と同一符号は同一又は相当部分を示すので説明を省略する。
図６に示す目標追尾装置は、追尾ゲート部１、平滑部６及び予測部７を備えている。
【0073】
平滑部６は、例えば、図７に示す平滑回路１４によって実現される。
平滑部６は、図１に示す平滑部２と同様に、追尾ゲート部１から、観測位置情報を取得し、予測部７から、追尾対象の状態の予測値を取得する。予測値は、観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、位置観測時点における追尾対象の状態の予測結果を示すものである。
平滑部６は、図１に示す平滑部２と同様に、観測位置情報と予測値とから、位置観測時点における追尾対象の状態を推定する。
平滑部６は、観測位置情報と予測値とから、運動モデルに含まれている加速度項を補正する
平滑部６は、追尾対象の状態の推定値を外部に出力する。平滑部６は、追尾対象の状態の推定値及び補正後の加速度項のそれぞれを予測部７に出力する。
平滑部６は、図１に示す平滑部２と同様に、追尾ゲート部１から、観測位置情報を取得し、予測部７から、追尾対象の状態の予測値を取得する。予測値は、観測位置が観測された時点である位置観測時点よりも１サンプリング前に予測された、位置観測時点における追尾対象の状態の予測結果を示すものである。
平滑部６は、図１に示す平滑部２と同様に、観測位置情報と予測値とから、位置観測時点における追尾対象の状態を推定する。
平滑部６は、観測位置情報と予測値とから、運動モデルに含まれている加速度項を補正する
平滑部６は、追尾対象の状態の推定値を外部に出力する。平滑部６は、追尾対象の状態の推定値及び補正後の加速度項のそれぞれを予測部７に出力する。
【0074】
予測部７は、例えば、図７に示す予測回路１５によって実現される。
予測部７は、Ｎ個の予測処理部４－１～４－Ｎと、予測値統合部８とを備えている。
予測部７は、平滑部６から、追尾対象の状態の推定値及び補正後の加速度項のそれぞれを取得する。
予測部７は、補正後の加速度項を含む追尾対象の運動モデルを用いて、平滑部６により推定された状態から、位置観測時点よりも１サンプリング後の、追尾対象の状態を予測する。
予測部３は、追尾対象の状態の予測値を追尾ゲート部１及び平滑部６のそれぞれに出力する。
予測部７は、Ｎ個の予測処理部４－１～４－Ｎと、予測値統合部８とを備えている。
予測部７は、平滑部６から、追尾対象の状態の推定値及び補正後の加速度項のそれぞれを取得する。
予測部７は、補正後の加速度項を含む追尾対象の運動モデルを用いて、平滑部６により推定された状態から、位置観測時点よりも１サンプリング後の、追尾対象の状態を予測する。
予測部３は、追尾対象の状態の予測値を追尾ゲート部１及び平滑部６のそれぞれに出力する。
【0075】
予測値統合部８は、予測処理部４－１～４－Ｎのそれぞれから予測値を取得する。
予測値統合部８は、Ｎ個の予測値を統合し、統合後の予測値を追尾ゲート部１及び平滑部６のそれぞれに出力する。
予測値統合部８は、Ｎ個の予測値を統合し、統合後の予測値を追尾ゲート部１及び平滑部６のそれぞれに出力する。
【0076】
図６では、目標追尾装置の構成要素である追尾ゲート部１、平滑部６及び予測部７のそれぞれが、図７に示すような専用のハードウェアによって実現されるものを想定している。即ち、目標追尾装置が、追尾ゲート回路１１、平滑回路１４及び予測回路１５によって実現されるものを想定している。
追尾ゲート回路１１、平滑回路１４及び予測回路１５のそれぞれは、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ、ＦＰＧＡ、又は、これらを組み合わせたものが該当する。
追尾ゲート回路１１、平滑回路１４及び予測回路１５のそれぞれは、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ、ＦＰＧＡ、又は、これらを組み合わせたものが該当する。
【0077】
目標追尾装置の構成要素は、専用のハードウェアによって実現されるものに限るものではなく、目標追尾装置が、ソフトウェア、ファームウェア、又は、ソフトウェアとファームウェアとの組み合わせによって実現されるものであってもよい。
目標追尾装置が、ソフトウェア又はファームウェア等によって実現される場合、追尾ゲート部１、平滑部６及び予測部７におけるそれぞれの処理手順をコンピュータに実行させるためのプログラムが図３に示すメモリ３１に格納される。そして、図３に示すプロセッサ３２がメモリ３１に格納されているプログラムを実行する。
目標追尾装置が、ソフトウェア又はファームウェア等によって実現される場合、追尾ゲート部１、平滑部６及び予測部７におけるそれぞれの処理手順をコンピュータに実行させるためのプログラムが図３に示すメモリ３１に格納される。そして、図３に示すプロセッサ３２がメモリ３１に格納されているプログラムを実行する。
【0078】
また、図７では、目標追尾装置の構成要素のそれぞれが専用のハードウェアによって実現される例を示し、図３では、目標追尾装置がソフトウェア又はファームウェア等によって実現される例を示している。しかし、これは一例に過ぎず、目標追尾装置における一部の構成要素が専用のハードウェアによって実現され、残りの構成要素がソフトウェア又はファームウェア等によって実現されるものであってもよい。
【0079】
【0080】
平滑部６は、追尾ゲート部１から、観測位置情報として、観測ベクトルｚｋを取得し、予測値統合部８から、予測ベクトルＸハットｋ（－）を取得する。
平滑部６は、以下の式（４７）に示すｘｋ（－）として、予測ベクトルＸハットｋ（－）を式（４７）に代入し、観測ベクトルｚｋを式（４７）に代入することで、イノベーションベクトルΔｋを算出する。
平滑部６は、以下の式（４７）に示すｘｋ（－）として、予測ベクトルＸハットｋ（－）を式（４７）に代入し、観測ベクトルｚｋを式（４７）に代入することで、イノベーションベクトルΔｋを算出する。
【0081】
【0082】
平滑部６は、予測値統合部８から、予測誤差共分散行列Ｐｋ－１（－）を取得する。
平滑部６は、以下の式（４８）に示すＰチルダｋ－１（－）として、予測誤差共分散行列Ｐｋ－１（－）を式（４８）に代入し、イノベーションベクトルΔｋを式（４８）に代入することで、予測残差共分散行列Ｖｋを算出する。
明細書の文章中では、電子出願の関係上、文字Ｐｋ－１（－）の上に“～”の記号を付することができないため、Ｐチルダｋ－１（－）のように表記している。
平滑部６は、以下の式（４８）に示すＰチルダｋ－１（－）として、予測誤差共分散行列Ｐｋ－１（－）を式（４８）に代入し、イノベーションベクトルΔｋを式（４８）に代入することで、予測残差共分散行列Ｖｋを算出する。
明細書の文章中では、電子出願の関係上、文字Ｐｋ－１（－）の上に“～”の記号を付することができないため、Ｐチルダｋ－１（－）のように表記している。
【0083】
【0084】
また、平滑部６は、以下の式（４９）に示すＰチルダｋ－１（－）として、予測誤差共分散行列Ｐｋ－１（－）を式（４９）に代入することで、予測位置誤差共分散行列Ｍｋを算出する。
平滑部６は、以下の式（５０）に示すように、予測残差共分散行列Ｖｋから予測位置誤差共分散行列Ｍｋを減算することで、予測残差共分散行列Ｎｋを算出する。
平滑部６は、以下の式（５０）に示すように、予測残差共分散行列Ｖｋから予測位置誤差共分散行列Ｍｋを減算することで、予測残差共分散行列Ｎｋを算出する。
【0085】
【0086】
平滑部６は、以下の式（５１）に示すように、予測位置誤差共分散行列Ｍｋの対角成分であるＭ（ｘ，ｙ，ｚ）と、予測残差共分散行列Ｎｋの対角成分であるＮ（ｘ，ｙ，ｚ）とから、直交座標系を構成する３軸におけるそれぞれの遅れ量に対応する定数加速度ベクトルｕｋ－１（－）の修正係数λｋ（ｘ，ｙ，ｚ）を算出する。
【0087】
【0088】
平滑部６は、以下の式（５２）に示すように、定数加速度ベクトルｕｋ－１（－）の修正係数λｋ（ｘ，ｙ，ｚ）を用いて、定数加速度ベクトルｕｋ－１（－）を補正する。ｕｋ’（－）は、補正後の定数加速度ベクトルである。
【0089】
【0090】
予測部７は、補正後の定数加速度ベクトルｕｋ’（－）を含む追尾対象の運動モデルを用いて、平滑部６により推定された状態から追尾対象の状態を予測する。
予測部７の予測値統合部８は、予測処理部４－１～４－Ｎのそれぞれから、追尾対象の状態の予測値として、Ｎ個の予測ベクトルＸハットｋ，ａ（－）を取得し、Ｎ個の予測ベクトルＸハットｋ，ａ（－）を統合する。
予測部７の予測値統合部８は、予測処理部４－１～４－Ｎのそれぞれから、追尾対象の状態の予測値として、Ｎ個の予測ベクトルＸハットｋ，ａ（－）を取得し、Ｎ個の予測ベクトルＸハットｋ，ａ（－）を統合する。
【0091】
即ち、予測値統合部８は、補正後の定数加速度ベクトルｕｋ’（－）を以下の式（５４）に代入し、式（５４）に示すｘｋ－１（＋）として、平滑ベクトルＸハットｋ，ａ（＋）又は平滑ベクトルＸハットｋ（＋）を式（５４）に代入することで、統合後の予測値である予測ベクトルＸハットｋ（－）を算出する。
また、予測値統合部８は、補正後の定数加速度ベクトルｕｋ’（－）を以下の式（５５）に代入し、予測処理部４－１～４－Ｎのそれぞれから出力された予測誤差共分散行列Ｐｋ，ａ（－）を式（５５）に代入することで、統合後の予測誤差共分散行列Ｐｋ（－）を算出する。
また、予測値統合部８は、補正後の定数加速度ベクトルｕｋ’（－）を以下の式（５５）に代入し、予測処理部４－１～４－Ｎのそれぞれから出力された予測誤差共分散行列Ｐｋ，ａ（－）を式（５５）に代入することで、統合後の予測誤差共分散行列Ｐｋ（－）を算出する。
【0092】
【0093】
以上の実施の形態２では、平滑部６が、観測位置情報と、位置観測時点における追尾対象の状態の予測値とから、運動モデルに含まれている加速度項を補正するように、図６に示す目標追尾装置を構成した。したがって、図６に示す目標追尾装置は、図１に示す目標追尾装置と同様に、追尾対象の機動方向を考慮した追尾対象の状態予測を行うことができるほか、図１に示す目標追尾装置よりも、平滑部２の推定精度を高めることができる。
【0094】
なお、本開示は、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【産業上の利用可能性】
【0095】
本開示は、目標追尾装置及び目標追尾方法に適している。
【符号の説明】
【0096】
１ 追尾ゲート部、２ 平滑部、３ 予測部、４－１～４－Ｎ 予測処理部、５ 予測値統合部、６ 平滑部、７ 予測部、８ 予測値統合部、１１ 追尾ゲート回路、１２ 平滑回路、１３ 予測回路、１４ 平滑回路、１５ 予測回路、３１ メモリ、３２ プロセッサ。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】