JP3218181B2 - センサのバイアス誤差推定装置 - Google Patents
センサのバイアス誤差推定装置Info
- Publication number
- JP3218181B2 JP3218181B2 JP10143396A JP10143396A JP3218181B2 JP 3218181 B2 JP3218181 B2 JP 3218181B2 JP 10143396 A JP10143396 A JP 10143396A JP 10143396 A JP10143396 A JP 10143396A JP 3218181 B2 JP3218181 B2 JP 3218181B2
- Authority
- JP
- Japan
- Prior art keywords
- matrix
- sensor
- target
- bias error
- observation
- 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
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Description
【０００１】
【発明の属する技術分野】この発明は、複数のセンサを
持つ目標位置計測装置において各々のセンサのバイアス
誤差を推定する装置に関するものである。
持つ目標位置計測装置において各々のセンサのバイアス
誤差を推定する装置に関するものである。
【０００２】
【従来の技術】図２５は、例えば特開平６−９４８２３
号公報に示された従来のセンサのバイアス誤差推定装置
を示す図である。図において、１は第１のセンサ、２は
第２のセンサ、３は第１の観測器、４は第２の観測器、
５は第１のセンサ１と第２のセンサ２の位置を入力する
センサ位置設定器、６は第１の観測器３からの目標位置
とセンサ位置設定器５からの第１のセンサ位置を加算し
て目標の観測位置を算出する第１の加算器、７は第２の
観測器４からの目標位置とセンサ位置設定器５からの第
２のセンサ位置を加算して目標の観測位置を算出する第
２の加算器、８は第１のセンサ１のバイアス誤差の初期
値を定める初期バイアス設定器、９は第１の加算器６と
第２の加算器７からの目標観測位置と初期バイアス設定
器８からのバイアス誤差初期値とからｋ個の目標の観測
行列を算出する観測行列生成器、１１は観測行列からバ
イアス誤差の推定値の共分散行列を算出する推定値評価
器、１２は観測行列と推定値評価器１１からのバイアス
誤差の推定値の共分散行列とからサンプリング時刻ｔに
おけるセンサのバイアス誤差推定値の仮の値を算出する
推定値算出器、１３は初期バイアス設定器８からのセン
サのバイアス誤差の初期値より推定値算出器１２からの
サンプリング時刻ｔにおけるセンサのバイアス誤差推定
値の仮の値を差し引く減算器、１４は減算器１３からの
第１のセンサと第２のセンサの時刻t におけるバイアス
誤差推定値１４を記憶しておく推定値記憶器である。
号公報に示された従来のセンサのバイアス誤差推定装置
を示す図である。図において、１は第１のセンサ、２は
第２のセンサ、３は第１の観測器、４は第２の観測器、
５は第１のセンサ１と第２のセンサ２の位置を入力する
センサ位置設定器、６は第１の観測器３からの目標位置
とセンサ位置設定器５からの第１のセンサ位置を加算し
て目標の観測位置を算出する第１の加算器、７は第２の
観測器４からの目標位置とセンサ位置設定器５からの第
２のセンサ位置を加算して目標の観測位置を算出する第
２の加算器、８は第１のセンサ１のバイアス誤差の初期
値を定める初期バイアス設定器、９は第１の加算器６と
第２の加算器７からの目標観測位置と初期バイアス設定
器８からのバイアス誤差初期値とからｋ個の目標の観測
行列を算出する観測行列生成器、１１は観測行列からバ
イアス誤差の推定値の共分散行列を算出する推定値評価
器、１２は観測行列と推定値評価器１１からのバイアス
誤差の推定値の共分散行列とからサンプリング時刻ｔに
おけるセンサのバイアス誤差推定値の仮の値を算出する
推定値算出器、１３は初期バイアス設定器８からのセン
サのバイアス誤差の初期値より推定値算出器１２からの
サンプリング時刻ｔにおけるセンサのバイアス誤差推定
値の仮の値を差し引く減算器、１４は減算器１３からの
第１のセンサと第２のセンサの時刻t におけるバイアス
誤差推定値１４を記憶しておく推定値記憶器である。
【０００３】次に原理について説明する。第１のセンサ
１、及び、第２のセンサ２からｋ個の目標を見たときの
ｋ番目の目標の正しい位置ベクトルは以下の式（１）の
ように極座標で表わされる。また、実際に各々のセンサ
から得られる観測位置ベクトルは以下の式（２）のよう
に極座標で表わされる。また、第１のセンサ１と第２の
センサ２のバイアス誤差を式（３）として、第１のセン
サ１の設置位置を原点、第２のセンサ２の位置ベクトル
を以下の式（４）のように直交座標で表わす。
１、及び、第２のセンサ２からｋ個の目標を見たときの
ｋ番目の目標の正しい位置ベクトルは以下の式（１）の
ように極座標で表わされる。また、実際に各々のセンサ
から得られる観測位置ベクトルは以下の式（２）のよう
に極座標で表わされる。また、第１のセンサ１と第２の
センサ２のバイアス誤差を式（３）として、第１のセン
サ１の設置位置を原点、第２のセンサ２の位置ベクトル
を以下の式（４）のように直交座標で表わす。
【０００４】
【数１】
【０００５】極座標で表わされた目標の正しい位置ベク
トルと、実際に各々のセンサから得られる観測位置ベク
トルを直交座標で表わすと各々次式（５）、（６）とな
る。また、明らかに式（７）が成り立つ。
トルと、実際に各々のセンサから得られる観測位置ベク
トルを直交座標で表わすと各々次式（５）、（６）とな
る。また、明らかに式（７）が成り立つ。
【０００６】
【数２】
【０００７】ここで関数Ｇとして式（８）を定義して、
第１のセンサ１と第２のセンサ２のバイアス誤差の１サ
ンプリング前の推定値として式（９）を関数G の平衡点
の近傍に選んで、関数Ｇを（Ｐ1ko アンダーバー
（ｔ），Ｐ2ko アンダーバー（ｔ），θ1■，θ2■）の
まわりでテイラー展開し、２次以上の項を無視すると、
式（１０）が得られる。ただし、式（１１）とする。
第１のセンサ１と第２のセンサ２のバイアス誤差の１サ
ンプリング前の推定値として式（９）を関数G の平衡点
の近傍に選んで、関数Ｇを（Ｐ1ko アンダーバー
（ｔ），Ｐ2ko アンダーバー（ｔ），θ1■，θ2■）の
まわりでテイラー展開し、２次以上の項を無視すると、
式（１０）が得られる。ただし、式（１１）とする。
【０００８】
【数３】
【０００９】関数Ｇは第１のセンサ１と第２のセンサ２
の観測ベクトルＰ1ko アンダーバー（ｔ）、Ｐ2ko アン
ダーバー（ｔ）に誤差がなければ恒等的に零になる。従
って、式（１０）の右辺を零とおいて、式（１２）が得
られる。
の観測ベクトルＰ1ko アンダーバー（ｔ）、Ｐ2ko アン
ダーバー（ｔ）に誤差がなければ恒等的に零になる。従
って、式（１０）の右辺を零とおいて、式（１２）が得
られる。
【００１０】
【数４】
【００１１】式（１２）の左辺は、バイアス誤差の初期
値と観測位置により定まる観測値と考えられる。式（１
２）の右辺第１項、第２項は求めるべきバイアス誤差を
状態変数として、観測行列との積と考えられる、第３
項、第４項は観測位置誤差と観測行列の積、すなわち、
雑音と考えられる。従って、式（１２）は観測系に雑音
が含まれる線形状態方程式と考えることができる。すな
わち、観測値をＺt アンダーバー、観測行列をＨt 、求
める状態変数をＸアンダーバー、観測雑音ベクトルをν
t アンダーバーとおけば、次式（１３）が得られる。こ
こで各項は式（１４）ないし（１８）で表される。
値と観測位置により定まる観測値と考えられる。式（１
２）の右辺第１項、第２項は求めるべきバイアス誤差を
状態変数として、観測行列との積と考えられる、第３
項、第４項は観測位置誤差と観測行列の積、すなわち、
雑音と考えられる。従って、式（１２）は観測系に雑音
が含まれる線形状態方程式と考えることができる。すな
わち、観測値をＺt アンダーバー、観測行列をＨt 、求
める状態変数をＸアンダーバー、観測雑音ベクトルをν
t アンダーバーとおけば、次式（１３）が得られる。こ
こで各項は式（１４）ないし（１８）で表される。
【００１２】
【数５】
【００１３】式（１３）の状態ベクトルＸアンダーバー
はサンプリング時刻によらない定数ベクトルなので、カ
ルマンフィルタの理論において推移行列を単位行列、駆
動雑音を零ベクトルとして、バイアス誤差を推定するこ
とができる。サンプリング時刻ｔにおける状態ベクトル
Ｘアンダーバーの推定結果を式（１９）と書けば、式
（２０）ないし（２２）となる。ここで、式（２３）な
いし（２８）が成立する。ここで、
はサンプリング時刻によらない定数ベクトルなので、カ
ルマンフィルタの理論において推移行列を単位行列、駆
動雑音を零ベクトルとして、バイアス誤差を推定するこ
とができる。サンプリング時刻ｔにおける状態ベクトル
Ｘアンダーバーの推定結果を式（１９）と書けば、式
（２０）ないし（２２）となる。ここで、式（２３）な
いし（２８）が成立する。ここで、
【００１４】
【数６】
【００１５】なお、（σ2 ikR（ｔ），σ2 ikE（ｔ），σ
2 ikAz （ｔ））は観測誤差の分散である。
2 ikAz （ｔ））は観測誤差の分散である。
【００１６】次に動作について説明する。図２５におい
て、第１のセンサ１、及び、第２のセンサ２は目標の位
置を観測するためのセンサで、同時に目標位置をサンプ
ルする。第１の観測器３、及び、第２の観測器４により
式（２）で表される目標の位置を出力する。また、セン
サ位置設定器５はある基準位置（以下基準位置と称す）
に対する第１のセンサ１と第２のセンサ２の位置を設定
する。ここで、第１のセンサ１と第２のセンサ２の位置
関係は式（７）で表される。第１の加算器６、及び、第
２の加算器７では、第１の観測器３、及び、第２の観測
器４からの目標の位置に第１のセンサ１と第２のセンサ
２の位置を加算し、基準位置に対する目標の観測位置を
算出する。また、初期バイアス設定器８は動作開始時
（ｔ＝１）では第１のセンサ１、及び、第２のセンサ２
のバイアス誤差初期値を設定し、それ以後（ｔ＞１）は
推定値記憶器１５からの前回算出のセンサのバイアス誤
差推定値１４を出力する（即ち式（２０）の右辺第１項
を出力する）。
て、第１のセンサ１、及び、第２のセンサ２は目標の位
置を観測するためのセンサで、同時に目標位置をサンプ
ルする。第１の観測器３、及び、第２の観測器４により
式（２）で表される目標の位置を出力する。また、セン
サ位置設定器５はある基準位置（以下基準位置と称す）
に対する第１のセンサ１と第２のセンサ２の位置を設定
する。ここで、第１のセンサ１と第２のセンサ２の位置
関係は式（７）で表される。第１の加算器６、及び、第
２の加算器７では、第１の観測器３、及び、第２の観測
器４からの目標の位置に第１のセンサ１と第２のセンサ
２の位置を加算し、基準位置に対する目標の観測位置を
算出する。また、初期バイアス設定器８は動作開始時
（ｔ＝１）では第１のセンサ１、及び、第２のセンサ２
のバイアス誤差初期値を設定し、それ以後（ｔ＞１）は
推定値記憶器１５からの前回算出のセンサのバイアス誤
差推定値１４を出力する（即ち式（２０）の右辺第１項
を出力する）。
【００１７】観測行列生成器９では、上記第１の加算器
６、及び、第２の加算器７からの目標の観測位置と、初
期バイアス設定器８からのバイアス誤差初期値または前
回算出のセンサのバイアス誤差推定値１４を受けて式
（１５）で表わされる観測行列Ｈt を演算して求める。
これは、式（１６）のＦk （ｔ）に相当する。推定値評
価器１１では式（２２）における共分散行列Ｐt を演算
する。さらに、推定値算出器１２では推定値評価器１１
からの推定値の共分散行列を用いて、サンプリング時刻
ｔにおけるバイアス誤差推定値の仮の値、すなわち式
（２０）の右辺第２項を算出する。さらに減算器１３で
推定値算出器１２からのバイアス誤差推定値を初期バイ
アス設定器８からのバイアス誤差初期値またはサンプリ
ング時刻（ｔ−１）のセンサのバイアス誤差推定値１
４、すなわち式（２０）の右辺第１項から差し引いて、
式（２０）のバイアス誤差推定値１４を演算し出力する
とともに、推定値記憶器１５に送出する。推定値記憶器
１５は、算出したサンプリング時刻ｔにおけるセンサの
バイアス誤差の推定値を記憶するとともに、前回算出の
センサのバイアス誤差推定値として初期バイアス誤差推
定器８に送出する。以後、この一連の処理をバイアス誤
差の推定値が収束するまで繰り返す。なお、このとき初
期バイアス誤差推定器８からはその都度前回（サンプリ
ング時刻（ｔ−１））算出のセンサのバイアス誤差推定
値が出力される。
６、及び、第２の加算器７からの目標の観測位置と、初
期バイアス設定器８からのバイアス誤差初期値または前
回算出のセンサのバイアス誤差推定値１４を受けて式
（１５）で表わされる観測行列Ｈt を演算して求める。
これは、式（１６）のＦk （ｔ）に相当する。推定値評
価器１１では式（２２）における共分散行列Ｐt を演算
する。さらに、推定値算出器１２では推定値評価器１１
からの推定値の共分散行列を用いて、サンプリング時刻
ｔにおけるバイアス誤差推定値の仮の値、すなわち式
（２０）の右辺第２項を算出する。さらに減算器１３で
推定値算出器１２からのバイアス誤差推定値を初期バイ
アス設定器８からのバイアス誤差初期値またはサンプリ
ング時刻（ｔ−１）のセンサのバイアス誤差推定値１
４、すなわち式（２０）の右辺第１項から差し引いて、
式（２０）のバイアス誤差推定値１４を演算し出力する
とともに、推定値記憶器１５に送出する。推定値記憶器
１５は、算出したサンプリング時刻ｔにおけるセンサの
バイアス誤差の推定値を記憶するとともに、前回算出の
センサのバイアス誤差推定値として初期バイアス誤差推
定器８に送出する。以後、この一連の処理をバイアス誤
差の推定値が収束するまで繰り返す。なお、このとき初
期バイアス誤差推定器８からはその都度前回（サンプリ
ング時刻（ｔ−１））算出のセンサのバイアス誤差推定
値が出力される。
【００１８】
【発明が解決しようとする課題】従来のセンサのバイア
ス誤差推定装置では、Ｐt を求める式（２０）の［］内
の第２項が正則であれば、センサのバイアス誤差推定
値、すなわち状態方程式の式（２０）は算出可能であ
る。そのための必要十分条件は、観測行列の階数が６で
あればいい。いいかえれば、２つ以上の目標を観測すれ
ばいいことになる。この条件は制御理論の可観測が対応
しているが、可観測の議論では観測行列が定数行列であ
ることを前提としている。ところが、従来のセンサのバ
イアス誤差推定装置では、観測行列の階数は、目標の観
測位置および１サンプリング前のセンサのバイアス誤差
推定値の不連続関数である。従って、観測行列の階数が
５以下の状況下でも、目標の観測位置等の若干の違いに
より観測行列の階数が６となり、バイアス誤差は推定で
きるものの、真値とかけ離れてしまう現象が発生すると
いう課題があった。一方、これ以外に、目標とセンサの
距離が２つの目標の距離に比べて大きい場合や２つのセ
ンサの間隔に比べて大きい場合、もしくは、仰角一定
等、センサと目標観測位置の関係に規則性がある場合
に、推定値評価器１１の式（２２）の共分散行列Ｐt の
演算で０割に近い現象が発生して、センサのバイアス誤
差推定値が真値とかけ離れてしまう現象が発生するとい
う課題があった。また、そうでなくても目標観測位置に
よりバイアス誤差の推定精度が劣化するという課題があ
った。
ス誤差推定装置では、Ｐt を求める式（２０）の［］内
の第２項が正則であれば、センサのバイアス誤差推定
値、すなわち状態方程式の式（２０）は算出可能であ
る。そのための必要十分条件は、観測行列の階数が６で
あればいい。いいかえれば、２つ以上の目標を観測すれ
ばいいことになる。この条件は制御理論の可観測が対応
しているが、可観測の議論では観測行列が定数行列であ
ることを前提としている。ところが、従来のセンサのバ
イアス誤差推定装置では、観測行列の階数は、目標の観
測位置および１サンプリング前のセンサのバイアス誤差
推定値の不連続関数である。従って、観測行列の階数が
５以下の状況下でも、目標の観測位置等の若干の違いに
より観測行列の階数が６となり、バイアス誤差は推定で
きるものの、真値とかけ離れてしまう現象が発生すると
いう課題があった。一方、これ以外に、目標とセンサの
距離が２つの目標の距離に比べて大きい場合や２つのセ
ンサの間隔に比べて大きい場合、もしくは、仰角一定
等、センサと目標観測位置の関係に規則性がある場合
に、推定値評価器１１の式（２２）の共分散行列Ｐt の
演算で０割に近い現象が発生して、センサのバイアス誤
差推定値が真値とかけ離れてしまう現象が発生するとい
う課題があった。また、そうでなくても目標観測位置に
よりバイアス誤差の推定精度が劣化するという課題があ
った。
【００１９】この発明は上記のような課題を解決するた
めになされたもので、最初に位置情報が既知の複数の仮
の可動目標を用いて第１のセンサ及び第２のセンサの各
々の推定演算を発散させずに、精度よくバイアス誤差を
求めることを目的とする。
めになされたもので、最初に位置情報が既知の複数の仮
の可動目標を用いて第１のセンサ及び第２のセンサの各
々の推定演算を発散させずに、精度よくバイアス誤差を
求めることを目的とする。
【００２０】
【課題を解決するための手段】この発明に係るセンサの
バイアス誤差推定装置は、第１のセンサによる目標から
のデータ入力により観測する第１の観測器と、この第１
のセンサとは離れた位置にある第２のセンサからのデー
タ入力により観測する第２の観測器とからの各時間毎の
出力で観測行列を得る観測行列生成器と、この観測行列
の誤差分散行列の仮の値の固有値を算出する誤差分散行
列の固有値算出器と、観測行列の誤差分散行列の仮の値
の最小固有値と、ｋ個の可動目標に搭載された位置計測
手段からの固定目標との相対位置データとを対応させて
時系列で記憶する目標物の相対位置記憶器と、この目標
物の相対位置記憶器で記憶された時系列の仮の値の最小
固有値の中で所定の値を取る固有値に対応する相対目標
位置データを出力する目標物の位置評価器と、目標物の
位置評価器で得られた相対目標位置データを上記ｋ個の
可動目標に送信して可動目標の位置を指示する目標位置
指示器と、観測行列生成器出力の観測行列出力と観測雑
音の共分散とからバイアス誤差推定値の共分散行列を算
出する推定値評価器と、この推定値評価器出力と観測行
列出力とから仮のバイアス誤差値を算出する推定値算出
器とを備えて、累積演算結果からセンサのバイアス誤差
を推定するようにした。
バイアス誤差推定装置は、第１のセンサによる目標から
のデータ入力により観測する第１の観測器と、この第１
のセンサとは離れた位置にある第２のセンサからのデー
タ入力により観測する第２の観測器とからの各時間毎の
出力で観測行列を得る観測行列生成器と、この観測行列
の誤差分散行列の仮の値の固有値を算出する誤差分散行
列の固有値算出器と、観測行列の誤差分散行列の仮の値
の最小固有値と、ｋ個の可動目標に搭載された位置計測
手段からの固定目標との相対位置データとを対応させて
時系列で記憶する目標物の相対位置記憶器と、この目標
物の相対位置記憶器で記憶された時系列の仮の値の最小
固有値の中で所定の値を取る固有値に対応する相対目標
位置データを出力する目標物の位置評価器と、目標物の
位置評価器で得られた相対目標位置データを上記ｋ個の
可動目標に送信して可動目標の位置を指示する目標位置
指示器と、観測行列生成器出力の観測行列出力と観測雑
音の共分散とからバイアス誤差推定値の共分散行列を算
出する推定値評価器と、この推定値評価器出力と観測行
列出力とから仮のバイアス誤差値を算出する推定値算出
器とを備えて、累積演算結果からセンサのバイアス誤差
を推定するようにした。
【００２１】また、誤差分散行列の固有値算出器に替え
て、観測行列生成器出力から観測行列の各要素に連続な
関数である観測行列とその転置行列との積からなる行列
の最小固有値を算出する階数評価値算出器を備えて、演
算結果からセンサのバイアス誤差を推定するようにし
た。
て、観測行列生成器出力から観測行列の各要素に連続な
関数である観測行列とその転置行列との積からなる行列
の最小固有値を算出する階数評価値算出器を備えて、演
算結果からセンサのバイアス誤差を推定するようにし
た。
【００２２】また、観測行列生成器出力から観測行列の
各要素に連続な関数である観測行列とその転置行列との
積からなる行列の最小固有値を算出する階数評価値算出
器を備え、誤差分散行列の固有値算出器と併用した。
各要素に連続な関数である観測行列とその転置行列との
積からなる行列の最小固有値を算出する階数評価値算出
器を備え、誤差分散行列の固有値算出器と併用した。
【００２３】また、推定値評価器と推定値算出器とに替
えて、観測行列生成器出力の観測行列から観測行列の階
数減少によるバイアス誤差の推定値の発散を抑えるペナ
ルティ関数を用いてバイアス誤差を推定するペナルティ
関数による推定値算出器を備えて、演算結果からセンサ
のバイアス誤差を推定するようにした。
えて、観測行列生成器出力の観測行列から観測行列の階
数減少によるバイアス誤差の推定値の発散を抑えるペナ
ルティ関数を用いてバイアス誤差を推定するペナルティ
関数による推定値算出器を備えて、演算結果からセンサ
のバイアス誤差を推定するようにした。
【００２４】また、推定値評価器と推定値算出器とに替
えて、観測行列生成器出力の観測行列と観測雑音の共分
散とからバイアス誤差の推定値の発散を抑えるペナルテ
ィ関数を用いてバイアス誤差の推定値の共分散行列を算
出するペナルティ関数型推定値評価器と、観測行列とペ
ナルティ関数型推定値評価器出力とからバイアス誤差推
定値の発散を抑えるペナルティ関数を用いて再帰的にセ
ンサのバイアス誤差を算出するペナルティ関数型再帰的
推定値算出器を備えて、累積演算結果からセンサのバイ
アス誤差を推定するようにした。
えて、観測行列生成器出力の観測行列と観測雑音の共分
散とからバイアス誤差の推定値の発散を抑えるペナルテ
ィ関数を用いてバイアス誤差の推定値の共分散行列を算
出するペナルティ関数型推定値評価器と、観測行列とペ
ナルティ関数型推定値評価器出力とからバイアス誤差推
定値の発散を抑えるペナルティ関数を用いて再帰的にセ
ンサのバイアス誤差を算出するペナルティ関数型再帰的
推定値算出器を備えて、累積演算結果からセンサのバイ
アス誤差を推定するようにした。
【００２５】また、目標物の位置記憶器に替えて、ｋ個
の可動目標に搭載された自己位置測定器からの自己位置
データを、誤差分散行列との仮の値の最小固有値と対応
させて時系列で記憶する可動目標の位置記憶器を備え
て、観測行列生成器出力を用いての演算結果からセンサ
のバイアス誤差を推定するようにした。
の可動目標に搭載された自己位置測定器からの自己位置
データを、誤差分散行列との仮の値の最小固有値と対応
させて時系列で記憶する可動目標の位置記憶器を備え
て、観測行列生成器出力を用いての演算結果からセンサ
のバイアス誤差を推定するようにした。
【００２６】また、目標物の位置記憶器に替えて、ｋ個
の可動目標に搭載された自己位置測定器からの自己位置
データを、誤差分散行列との仮の値の最小固有値と対応
させて時系列で記憶する可動目標の位置記憶器を備え
て、観測行列生成器出力を用いての演算結果からセンサ
のバイアス誤差を推定するようにした。
の可動目標に搭載された自己位置測定器からの自己位置
データを、誤差分散行列との仮の値の最小固有値と対応
させて時系列で記憶する可動目標の位置記憶器を備え
て、観測行列生成器出力を用いての演算結果からセンサ
のバイアス誤差を推定するようにした。
【００２７】また、目標物の位置記憶器に替えて、ｋ個
の可動目標に搭載された自己位置測定器からの自己位置
データを、誤差分散行列との仮の値の最小固有値と対応
させて時系列で記憶する可動目標の位置記憶器を備え
て、観測行列生成器出力を用いての演算結果からセンサ
のバイアス誤差を推定するようにした。
の可動目標に搭載された自己位置測定器からの自己位置
データを、誤差分散行列との仮の値の最小固有値と対応
させて時系列で記憶する可動目標の位置記憶器を備え
て、観測行列生成器出力を用いての演算結果からセンサ
のバイアス誤差を推定するようにした。
【００２８】また更に、自己位置データは、ｋ個の可動
目標に搭載された自己位置測定器のＧＰＳ(Globl Posit
ioning System)装置により得られる構成とした。
目標に搭載された自己位置測定器のＧＰＳ(Globl Posit
ioning System)装置により得られる構成とした。
【００２９】
【発明の実施の形態】実施の形態１． 図１はこの発明の実施の形態１のセンサのバイアス誤差
推定装置の構成図である。図中１〜９及び１１〜１５は
従来の装置と同一である。１０はサンプリング時刻t に
おける観測行列の誤差分散行列の仮の値の最小固有値を
算出する誤差分散行列の固有値算出器、２５ａ、２５
ｂ、２５ｃは静止した鉄塔、建物等の他の目標との相対
位置を計測する各可動目標１、可動目標２ないし可動目
標Ｋに搭載された位置計測装置、２６ａ、２６ｂ、２６
ｃは位置観測装置２５ａ、２５ｂ、２５ｃで計測した固
定目標物との相対位置データを第２の通信装置２７に送
信する、もしくは第２の通信装置２７から固定目標物の
位置データを受信する第１の通信装置、２７は第１の通
信装置２６ａ、２６ｂ、２６ｃから送られてくる固定目
標物との相対位置データを受信する、もしくは固定目標
物との相対位置データを第１の通信装置に送信する第２
の通信装置、１６は誤差分散行列の固有値算出器１０で
算出した観測行列の誤差分散行列の仮の値の最小固有値
と、第２の通信装置２７で受信した固定目標物との相対
位置データとをペアにして時系列で記憶する目標物の相
対位置記憶器、１７は目標物の相対位置記憶器１６で記
憶した各サンプリング時刻における誤差分散行列の仮の
値の最小固有値と固定目標物との相対位置データのなか
で、最小固有値がある値以上となる固定目標物との相対
位置データを出力する目標物の位置評価器、１８は目標
物の位置評価器１７が出力した固定目標物との相対位置
データを第２の通信装置を介してｋ個の可動目標それぞ
れに送信する目標位置指示器である。また、図２は誤差
分散行列の固有値算出器１０の処理の流れを示すフロー
チャートである。
推定装置の構成図である。図中１〜９及び１１〜１５は
従来の装置と同一である。１０はサンプリング時刻t に
おける観測行列の誤差分散行列の仮の値の最小固有値を
算出する誤差分散行列の固有値算出器、２５ａ、２５
ｂ、２５ｃは静止した鉄塔、建物等の他の目標との相対
位置を計測する各可動目標１、可動目標２ないし可動目
標Ｋに搭載された位置計測装置、２６ａ、２６ｂ、２６
ｃは位置観測装置２５ａ、２５ｂ、２５ｃで計測した固
定目標物との相対位置データを第２の通信装置２７に送
信する、もしくは第２の通信装置２７から固定目標物の
位置データを受信する第１の通信装置、２７は第１の通
信装置２６ａ、２６ｂ、２６ｃから送られてくる固定目
標物との相対位置データを受信する、もしくは固定目標
物との相対位置データを第１の通信装置に送信する第２
の通信装置、１６は誤差分散行列の固有値算出器１０で
算出した観測行列の誤差分散行列の仮の値の最小固有値
と、第２の通信装置２７で受信した固定目標物との相対
位置データとをペアにして時系列で記憶する目標物の相
対位置記憶器、１７は目標物の相対位置記憶器１６で記
憶した各サンプリング時刻における誤差分散行列の仮の
値の最小固有値と固定目標物との相対位置データのなか
で、最小固有値がある値以上となる固定目標物との相対
位置データを出力する目標物の位置評価器、１８は目標
物の位置評価器１７が出力した固定目標物との相対位置
データを第２の通信装置を介してｋ個の可動目標それぞ
れに送信する目標位置指示器である。また、図２は誤差
分散行列の固有値算出器１０の処理の流れを示すフロー
チャートである。
【００３０】次に原理について説明する。第１、及び、
第２のセンサから目標の位置を観測してそのデータから
観測行列生成器９で目標の観測行列を生成する処理まで
は従来の装置と同一である。ここで、バイアス誤差推定
値Ｘt アンダーバーハットの収束性について考える。ま
ず、Ｘt アンダーバーハットが算出可能となるために
は、Ｒt ik が正値対称行列、Ｆik（ｔ）が正則行列、Ｐ
-1（_） が半正値対称行列となるので、次式（２９）が
正則ならば式（２０）〜式（２２）は一意的に解ける。
なお、ａt アンダーバーとして式（３０）とおくと、式
（３１）が得られる。
第２のセンサから目標の位置を観測してそのデータから
観測行列生成器９で目標の観測行列を生成する処理まで
は従来の装置と同一である。ここで、バイアス誤差推定
値Ｘt アンダーバーハットの収束性について考える。ま
ず、Ｘt アンダーバーハットが算出可能となるために
は、Ｒt ik が正値対称行列、Ｆik（ｔ）が正則行列、Ｐ
-1（_） が半正値対称行列となるので、次式（２９）が
正則ならば式（２０）〜式（２２）は一意的に解ける。
なお、ａt アンダーバーとして式（３０）とおくと、式
（３１）が得られる。
【００３１】
【数７】
【００３２】従って、式（２０）〜式（２２）は、次式
（３２）、（３３）となる。
（３２）、（３３）となる。
【００３３】
【数８】
【００３４】これは、式（２２）の演算で零割りに近い
事象が発生した場合、サンプリング時刻ｔにおける状態
ベクトルＸt アンダーバーの推定結果Ｘt アンダーバー
ハットが発散する可能性を示している。すなわち、理論
的にＡt が正則であっても、第１のセンサ１と第２のセ
ンサ２の距離が小さい場合や目標とセンサの距離が大き
い場合等のように、同じ目標に対する第１のセンサ１と
第２のセンサ２の観測値の差異がとても小さいときに、
計算機丸め誤差の蓄積により式（２２）の演算で零割り
に近い事象が発生してバイアス誤差の推定値が真値とか
け離れることがある。さて、行列Ａt のが小固有値をλ
t とした時、Ｐ-1（_） が半正値対称行列となるので、
式（３４）の関係が得られる。
事象が発生した場合、サンプリング時刻ｔにおける状態
ベクトルＸt アンダーバーの推定結果Ｘt アンダーバー
ハットが発散する可能性を示している。すなわち、理論
的にＡt が正則であっても、第１のセンサ１と第２のセ
ンサ２の距離が小さい場合や目標とセンサの距離が大き
い場合等のように、同じ目標に対する第１のセンサ１と
第２のセンサ２の観測値の差異がとても小さいときに、
計算機丸め誤差の蓄積により式（２２）の演算で零割り
に近い事象が発生してバイアス誤差の推定値が真値とか
け離れることがある。さて、行列Ａt のが小固有値をλ
t とした時、Ｐ-1（_） が半正値対称行列となるので、
式（３４）の関係が得られる。
【００３５】
【数９】
【００３６】ここで、||ａ||アンダーバーはベクトルａ
アンダーバーのユークリッドノルムを表わす。従って、
行列Ａt の最小固有値λt がある正の定数ａより大きい
時、式（２０）より、サンプリング時刻ｔにおける状態
ベクトルｘアンダーバーの推定結果、すなわち、バイア
ス誤差の推定値ｘt アンダーバーハットが発散せずに算
出できるので、行列Ａt の最小固有値λt が次の式（３
５）を満たすようにｋ個の目標を運動もしくは静止さ
せ、（２０）ないし式（２２）を用いて状態ベクトルｘ
アンダーバーの推定結果を反復計算する。
アンダーバーのユークリッドノルムを表わす。従って、
行列Ａt の最小固有値λt がある正の定数ａより大きい
時、式（２０）より、サンプリング時刻ｔにおける状態
ベクトルｘアンダーバーの推定結果、すなわち、バイア
ス誤差の推定値ｘt アンダーバーハットが発散せずに算
出できるので、行列Ａt の最小固有値λt が次の式（３
５）を満たすようにｋ個の目標を運動もしくは静止さ
せ、（２０）ないし式（２２）を用いて状態ベクトルｘ
アンダーバーの推定結果を反復計算する。
【００３７】次に、上記に説明した原理を適用した実施
の形態１の装置の動作を図１、および、図２にしたがっ
て説明する。図１において、第１のセンサ１、及び、第
２のセンサ２から目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。誤差分散行列の固有値算出器１０では、観測
行列の誤差分散行列の仮の値の最小固有値を算出する。
次に図２にしたがって、誤差分散行列の固有値算出器１
０の処理の流れを説明する。ステップ１００ではサンプ
ル時刻ｔにおける観測行列生成器９からの観測行列を取
り込む。ステップ１０１では、ステップ１００で取り込
んだ観測行列と、あらかじめ設定された第１のセンサ１
と第２のセンサ２の観測誤差の分散から、式（２９）を
用いて観測行列の誤差分散行列の仮の値である行列Ａt
を算出する。ステップ１０２では、ステップ１０１で算
出した行列Ａt の最小固有値を算出する。
の形態１の装置の動作を図１、および、図２にしたがっ
て説明する。図１において、第１のセンサ１、及び、第
２のセンサ２から目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。誤差分散行列の固有値算出器１０では、観測
行列の誤差分散行列の仮の値の最小固有値を算出する。
次に図２にしたがって、誤差分散行列の固有値算出器１
０の処理の流れを説明する。ステップ１００ではサンプ
ル時刻ｔにおける観測行列生成器９からの観測行列を取
り込む。ステップ１０１では、ステップ１００で取り込
んだ観測行列と、あらかじめ設定された第１のセンサ１
と第２のセンサ２の観測誤差の分散から、式（２９）を
用いて観測行列の誤差分散行列の仮の値である行列Ａt
を算出する。ステップ１０２では、ステップ１０１で算
出した行列Ａt の最小固有値を算出する。
【００３８】図１において、ｋ個の可動目標それぞれに
搭載している位置計測装置２５ａ、２５ｂ、２５ｃで
は、目標自らの位置を特定するために静止物体（他の固
定目標物）の位置を計測する。第１の通信装置２６ａ、
２６ｂ、２６ｃでは位置計測装置２５ａ、２５ｂ、２５
ｃで計測した上記固定目標物との相対位置データを第２
の通信装置２７に送信する。第２の通信装置２７では第
１の通信装置２６ａ、２６ｂ、２６ｃで送信した固定目
標物との相対位置データを受信して、目標物の相対位置
記憶器１６に出力する。目標物の相対位置記憶器１６で
は誤差分散行列の固有値算出器１０で算出した観測行列
の誤差分散行列の仮の値の最小固有値と、第２の通信装
置２７で受信した固定目標物との相対位置データとをサ
ンプル時刻毎に記憶する。目標物の相対位置評価器１７
では目標物の相対位置記憶器１６に記憶された最小固有
値が式（３５）を満たすときの固定目標物との相対位置
データを検索して出力する。ここで、ある定数ａの値は
計算機の精度やセンサの精度により決まる値である。目
標位置指示器１８では目標物の位置評価器１７で検索し
た固定目標物との相対位置データを第２の通信装置２７
を介して送出する。ｋ個の可動目標は、この通信装置２
７で送信された固定目標物との相対位置データを第１の
通信装置２６ａ、２６ｂ、２６ｃで受信し、各目標の操
縦者は位置計測装置２５ａ、２５ｂ、２５ｃで固定目標
物との相対位置を観測しながら、それが通信装置２６
ａ、２６ｂ、２６ｃで受信した固定目標物との相対位置
データと等しくなる位置もしくは航跡に各可動目標を移
動もしくは運動させる。その後は、この移動後には、複
数の可動目標からのある時刻、第１と第２のセンサへの
一斉の位置データにより本来の観測行列生成器は必要な
行列を生成し、従来の装置と同様に推定値評価器１１以
降の処理を行い、第１のセンサ１および第２のセンサ２
のバイアス誤差の推定値を算出する。
搭載している位置計測装置２５ａ、２５ｂ、２５ｃで
は、目標自らの位置を特定するために静止物体（他の固
定目標物）の位置を計測する。第１の通信装置２６ａ、
２６ｂ、２６ｃでは位置計測装置２５ａ、２５ｂ、２５
ｃで計測した上記固定目標物との相対位置データを第２
の通信装置２７に送信する。第２の通信装置２７では第
１の通信装置２６ａ、２６ｂ、２６ｃで送信した固定目
標物との相対位置データを受信して、目標物の相対位置
記憶器１６に出力する。目標物の相対位置記憶器１６で
は誤差分散行列の固有値算出器１０で算出した観測行列
の誤差分散行列の仮の値の最小固有値と、第２の通信装
置２７で受信した固定目標物との相対位置データとをサ
ンプル時刻毎に記憶する。目標物の相対位置評価器１７
では目標物の相対位置記憶器１６に記憶された最小固有
値が式（３５）を満たすときの固定目標物との相対位置
データを検索して出力する。ここで、ある定数ａの値は
計算機の精度やセンサの精度により決まる値である。目
標位置指示器１８では目標物の位置評価器１７で検索し
た固定目標物との相対位置データを第２の通信装置２７
を介して送出する。ｋ個の可動目標は、この通信装置２
７で送信された固定目標物との相対位置データを第１の
通信装置２６ａ、２６ｂ、２６ｃで受信し、各目標の操
縦者は位置計測装置２５ａ、２５ｂ、２５ｃで固定目標
物との相対位置を観測しながら、それが通信装置２６
ａ、２６ｂ、２６ｃで受信した固定目標物との相対位置
データと等しくなる位置もしくは航跡に各可動目標を移
動もしくは運動させる。その後は、この移動後には、複
数の可動目標からのある時刻、第１と第２のセンサへの
一斉の位置データにより本来の観測行列生成器は必要な
行列を生成し、従来の装置と同様に推定値評価器１１以
降の処理を行い、第１のセンサ１および第２のセンサ２
のバイアス誤差の推定値を算出する。
【００３９】このように、目標とセンサの幾何学的な位
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、誤差分散行列の正則が保たれるようにｋ個の
目標を配置した上でバイアス誤差の推定演算をしている
ので、可動目標１ないしＫの目標位置がフィードバック
されて定まると、その定まった各位置からの相対位置デ
ータにより、時間的には一時点のデータであっても第１
と第２のセンサの向きを定めるための観測行列が得られ
て、即ち、測定値評価器と推定値算出器により精度の良
いセンサのバイアス誤差推定値を得ることができる。
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、誤差分散行列の正則が保たれるようにｋ個の
目標を配置した上でバイアス誤差の推定演算をしている
ので、可動目標１ないしＫの目標位置がフィードバック
されて定まると、その定まった各位置からの相対位置デ
ータにより、時間的には一時点のデータであっても第１
と第２のセンサの向きを定めるための観測行列が得られ
て、即ち、測定値評価器と推定値算出器により精度の良
いセンサのバイアス誤差推定値を得ることができる。
【００４０】実施の形態２．誤差分散行列の固有値算出
器に換えて、観測行列の階数を評価してｋ個の可動目標
を配置させる装置を説明する。図３はこの発明の実施の
形態２を示すセンサのバイアス誤差推定装置の構成図で
ある。図中１〜９及び１１〜１５は従来の装置と同一で
ある。１９はサンプリングｔにおける観測行列の階数に
よる解析から得た判定式を算出する階数評価値算出器で
ある。また、１６〜１８、２５〜２７は実施の形態１と
同一である。また、図２は階数評価値算出器１９の処理
の流れを示すフローチャートである。
器に換えて、観測行列の階数を評価してｋ個の可動目標
を配置させる装置を説明する。図３はこの発明の実施の
形態２を示すセンサのバイアス誤差推定装置の構成図で
ある。図中１〜９及び１１〜１５は従来の装置と同一で
ある。１９はサンプリングｔにおける観測行列の階数に
よる解析から得た判定式を算出する階数評価値算出器で
ある。また、１６〜１８、２５〜２７は実施の形態１と
同一である。また、図２は階数評価値算出器１９の処理
の流れを示すフローチャートである。
【００４１】次に原理について説明する。第１、及び、
第２のセンサから目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。次に、実施の形態１とは別の観点で、ｘt ア
ンダーバーハットの収束性について考える。実施の形態
１で式（２９）が正則ならば式（２０）〜式（２２）が
一意的に解けることを示した。行列Ａt が正則であるた
めの必要十分条件は観測行列Ｈt の階数が６である。こ
れは、３次元センサの場合、少なくとも２つの目標の観
測値が必要であることを意味する。ところで、観測行列
Ｈt の階数が６の条件は、制御理論での可観測が対応し
ているが、可観測の議論では観測行列が定数行列である
ことを前提にしているのに対し、従来の装置の観測行列
は目標観測位置の関数となっている。したがって、観測
行列Ｈt の階数が５以下の状況下でも、目標の観測位置
等の若干の違いにより観測行列Ｈt の階数が６となり、
センサのバイアス誤差は推定できるものの、真値とかけ
離れてしまう現象が発生する。
第２のセンサから目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。次に、実施の形態１とは別の観点で、ｘt ア
ンダーバーハットの収束性について考える。実施の形態
１で式（２９）が正則ならば式（２０）〜式（２２）が
一意的に解けることを示した。行列Ａt が正則であるた
めの必要十分条件は観測行列Ｈt の階数が６である。こ
れは、３次元センサの場合、少なくとも２つの目標の観
測値が必要であることを意味する。ところで、観測行列
Ｈt の階数が６の条件は、制御理論での可観測が対応し
ているが、可観測の議論では観測行列が定数行列である
ことを前提にしているのに対し、従来の装置の観測行列
は目標観測位置の関数となっている。したがって、観測
行列Ｈt の階数が５以下の状況下でも、目標の観測位置
等の若干の違いにより観測行列Ｈt の階数が６となり、
センサのバイアス誤差は推定できるものの、真値とかけ
離れてしまう現象が発生する。
【００４２】このように、従来の装置は制御の一般論と
異なるので、階数に代わる解析指標が必要である。観測
行列Ｈt の階数は、ｋ個の目標の観測位置、及び、セン
サのバイアス誤差の１サンプリング前の推定値の不連続
関数である。そこで、行列の固有値は行列の要素の連続
関数であること、行列、観測Ｈt の階数は行列Ｈt TＨt
の階数と一致すること、および、正方行列の階数は０で
ない固有値の個数と一致することから、観測行列Ｈt の
各要素に連続な関数を解析指標とする。すなわち行列Ｈ
t Tとその転置行列であるＨt との積からなる行列Ｈt TＨ
t の最小固有値をαt 、ｃを正の定数とした時、式（３
６）を満たすサンプリング時に式（２０）〜式（２２）
を用いて状態ベクトルｘアンダーバーの推定結果を反復
計算すればよいことが分かる。従って、可動目標１ない
しＫからの相対位置データにより最小固有値αt を求め
る階数評価値算出器１９による演算を時間的に繰り返せ
ばよい。
異なるので、階数に代わる解析指標が必要である。観測
行列Ｈt の階数は、ｋ個の目標の観測位置、及び、セン
サのバイアス誤差の１サンプリング前の推定値の不連続
関数である。そこで、行列の固有値は行列の要素の連続
関数であること、行列、観測Ｈt の階数は行列Ｈt TＨt
の階数と一致すること、および、正方行列の階数は０で
ない固有値の個数と一致することから、観測行列Ｈt の
各要素に連続な関数を解析指標とする。すなわち行列Ｈ
t Tとその転置行列であるＨt との積からなる行列Ｈt TＨ
t の最小固有値をαt 、ｃを正の定数とした時、式（３
６）を満たすサンプリング時に式（２０）〜式（２２）
を用いて状態ベクトルｘアンダーバーの推定結果を反復
計算すればよいことが分かる。従って、可動目標１ない
しＫからの相対位置データにより最小固有値αt を求め
る階数評価値算出器１９による演算を時間的に繰り返せ
ばよい。
【００４３】次に、上記で説明した原理を適用した実施
の形態２の装置の動作を図３、および、図４にしたがっ
て説明する。図３において、第１のセンサ１、及び、第
２のセンサ２から目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。次に図４にしたがって、階数評価値算出器１
９の処理の流れを説明する。階数評価器算出器では、目
標とセンサの幾何学的位置関係により、観測行列の階数
がバイアス誤差推定値算出可能となるための条件を満た
さない事象を排除するための評価値を算出する。具体的
には以下のようになる。ステップ１００ではサンプル時
刻ｔにおける観測行列生成器９からの観測行列を取り込
む。ステップ１０３では、ステップ１００で取り込んだ
観測行列から、行列の積である行列Ｈt TＨt を算出す
る。ステップ１０４では、ステップ１０３で算出した行
列Ｈt TＨt の最小固有値を算出する。これは、行列の固
有値はその行列の各要素の連続関数であること、及び、
正方行列の階数は０でない固有値の個数と一致するとい
うことから、観測行列Ｈt と階数が等しく、かつ、正方
行列であるＨt TＨt の固有値を用いて観測行列Ｈt の階
数を評価している。
の形態２の装置の動作を図３、および、図４にしたがっ
て説明する。図３において、第１のセンサ１、及び、第
２のセンサ２から目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。次に図４にしたがって、階数評価値算出器１
９の処理の流れを説明する。階数評価器算出器では、目
標とセンサの幾何学的位置関係により、観測行列の階数
がバイアス誤差推定値算出可能となるための条件を満た
さない事象を排除するための評価値を算出する。具体的
には以下のようになる。ステップ１００ではサンプル時
刻ｔにおける観測行列生成器９からの観測行列を取り込
む。ステップ１０３では、ステップ１００で取り込んだ
観測行列から、行列の積である行列Ｈt TＨt を算出す
る。ステップ１０４では、ステップ１０３で算出した行
列Ｈt TＨt の最小固有値を算出する。これは、行列の固
有値はその行列の各要素の連続関数であること、及び、
正方行列の階数は０でない固有値の個数と一致するとい
うことから、観測行列Ｈt と階数が等しく、かつ、正方
行列であるＨt TＨt の固有値を用いて観測行列Ｈt の階
数を評価している。
【００４４】図３において、位置計測装置２５ａ、２５
ｂ、２５ｃで固定目標物との相対位置を計測する処理か
ら第２の通信装置２７で第１の通信装置２６ａ、２６
ｂ、２６ｃで送信した固定目標物との相対位置データを
受信して、目標物の相対位置記憶器１６に出力するまで
の処理は実施の形態１と同一である。目標物の相対位置
記憶器１６では階数評価値算出器１９で算出したＨt TＨ
t の最小固有値と、第２の通信装置２７で受信した固定
目標物との相対位置データとをサンプル時刻毎に記憶す
る。目標物の位置評価器１７では目標物の位置記憶器１
６に記憶された最小固有値が式（３６）を満たすときの
固定目標物との相対位置データを検索して出力する。目
標位置指示器１８では目標物の位置評価器１７で検索し
た固定目標物との相対位置データを第２の通信装置２７
を介して送出する。ｋ個の可動目標は、この通信装置２
７で送信された固定目標物との相対位置データを第１の
通信装置２６ａ、２６ｂ、２６ｃで受信し、各可動目標
の操縦者は位置計測装置２５ａ、２５ｂ、２５ｃで固定
目標物との相対位置を観測しながら、それが通信装置２
６ａ、２６ｂ、２６ｃで受信した固定目標物との相対位
置データと等しくなる位置もしくは航跡に各可動目標を
移動もしくは運動させる。可動目標１ないしＫの位置が
フィードバックで定まると、その定まった位置に対する
ある時刻の第１と第２のセンサによる一斉計測により、
その後は、従来の装置と同様に推定値評価器１１以降の
処理を行い、第１のセンサ１および第２のセンサ２のバ
イアス誤差の推定値を算出する。
ｂ、２５ｃで固定目標物との相対位置を計測する処理か
ら第２の通信装置２７で第１の通信装置２６ａ、２６
ｂ、２６ｃで送信した固定目標物との相対位置データを
受信して、目標物の相対位置記憶器１６に出力するまで
の処理は実施の形態１と同一である。目標物の相対位置
記憶器１６では階数評価値算出器１９で算出したＨt TＨ
t の最小固有値と、第２の通信装置２７で受信した固定
目標物との相対位置データとをサンプル時刻毎に記憶す
る。目標物の位置評価器１７では目標物の位置記憶器１
６に記憶された最小固有値が式（３６）を満たすときの
固定目標物との相対位置データを検索して出力する。目
標位置指示器１８では目標物の位置評価器１７で検索し
た固定目標物との相対位置データを第２の通信装置２７
を介して送出する。ｋ個の可動目標は、この通信装置２
７で送信された固定目標物との相対位置データを第１の
通信装置２６ａ、２６ｂ、２６ｃで受信し、各可動目標
の操縦者は位置計測装置２５ａ、２５ｂ、２５ｃで固定
目標物との相対位置を観測しながら、それが通信装置２
６ａ、２６ｂ、２６ｃで受信した固定目標物との相対位
置データと等しくなる位置もしくは航跡に各可動目標を
移動もしくは運動させる。可動目標１ないしＫの位置が
フィードバックで定まると、その定まった位置に対する
ある時刻の第１と第２のセンサによる一斉計測により、
その後は、従来の装置と同様に推定値評価器１１以降の
処理を行い、第１のセンサ１および第２のセンサ２のバ
イアス誤差の推定値を算出する。
【００４５】このように、観測行列の階数が５以下の状
況下でも、目標の観測位置等の若干の違いによりその階
数が６となってセンサのバイアス誤差推定値が真値とか
け離れてしまう現象を回避するように、観測行列の階数
を評価してｋ個の目標を配置した上でバイアス誤差の推
定演算をしているので、精度の良いセンサのバイアス誤
差推定値を得ることができる。
況下でも、目標の観測位置等の若干の違いによりその階
数が６となってセンサのバイアス誤差推定値が真値とか
け離れてしまう現象を回避するように、観測行列の階数
を評価してｋ個の目標を配置した上でバイアス誤差の推
定演算をしているので、精度の良いセンサのバイアス誤
差推定値を得ることができる。
【００４６】実施の形態３．実施の形態１と２を組み合
わせた場合を説明する。図５はこの発明の実施の形態３
のセンサのバイアス誤差推定装置の構成図である。図中
１〜９及び１１〜１５は従来の装置と、また、１０、１
６〜１８、２５〜２７は実施の形態１で、１９は実施の
形態２で用いられた要素とそれぞれ同一である。
わせた場合を説明する。図５はこの発明の実施の形態３
のセンサのバイアス誤差推定装置の構成図である。図中
１〜９及び１１〜１５は従来の装置と、また、１０、１
６〜１８、２５〜２７は実施の形態１で、１９は実施の
形態２で用いられた要素とそれぞれ同一である。
【００４７】次に、本実施の形態の装置の動作を図５に
従って説明する。第１、及び、第２のセンサから可動目
標の位置を観測し誤差分散行列の固有値算出器１０で行
列Ａt の最小固有値を算出する処理までは実施の形態１
と同一である。階数評価値算出器１９では、誤差分散行
列の固有値算出器１０と同時に観測行列を入力し、Ｈt T
Ｈt の最小固有値を算出する。ｋ個の可動目標からの固
定目標物との相対位置データの送信は、実施の形態１と
同様である。このデータに基づき、目標物の相対位置記
憶器１６では誤差分散行列の固有値算出器１０で算出し
た観測行列の誤差分散行列の仮の値の最小固有値と、階
数評価値算出器１９で算出したＨt TＨt の最小固有値
と、第２の通信装置２７で受信した固定目標物との相対
位置データとをペアにしてサンプル時刻毎に記憶する。
目標物の相対位置評価器１７では目標物の位置記憶器１
６に記憶された誤差分散行列の仮の値の最小固有値及び
Ｈt TＨt の最小固有値がそれぞれ式（３５）及び式を
（３６）を同時に満たすときの固定目標物との相対位置
データを検索して出力する。これ以降の目標位置指示器
１８による相対位置データの送信も実施の形態１と同様
である。
従って説明する。第１、及び、第２のセンサから可動目
標の位置を観測し誤差分散行列の固有値算出器１０で行
列Ａt の最小固有値を算出する処理までは実施の形態１
と同一である。階数評価値算出器１９では、誤差分散行
列の固有値算出器１０と同時に観測行列を入力し、Ｈt T
Ｈt の最小固有値を算出する。ｋ個の可動目標からの固
定目標物との相対位置データの送信は、実施の形態１と
同様である。このデータに基づき、目標物の相対位置記
憶器１６では誤差分散行列の固有値算出器１０で算出し
た観測行列の誤差分散行列の仮の値の最小固有値と、階
数評価値算出器１９で算出したＨt TＨt の最小固有値
と、第２の通信装置２７で受信した固定目標物との相対
位置データとをペアにしてサンプル時刻毎に記憶する。
目標物の相対位置評価器１７では目標物の位置記憶器１
６に記憶された誤差分散行列の仮の値の最小固有値及び
Ｈt TＨt の最小固有値がそれぞれ式（３５）及び式を
（３６）を同時に満たすときの固定目標物との相対位置
データを検索して出力する。これ以降の目標位置指示器
１８による相対位置データの送信も実施の形態１と同様
である。
【００４８】このように、目標とセンサの幾何学的な位
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、また、観測行列の階数が５以下の状況下で
も、目標の観測位置等の若干の違いによりその階数が６
となってセンサのバイアス誤差推定値が真値とかけ離れ
てしまう現象を回避するように、誤差分散行列の正則性
及び観測行列の階数を評価してｋ個の目標を配置した上
でバイアス誤差の推定演算をして、精度の良いセンサの
バイアス誤差推定をしている。
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、また、観測行列の階数が５以下の状況下で
も、目標の観測位置等の若干の違いによりその階数が６
となってセンサのバイアス誤差推定値が真値とかけ離れ
てしまう現象を回避するように、誤差分散行列の正則性
及び観測行列の階数を評価してｋ個の目標を配置した上
でバイアス誤差の推定演算をして、精度の良いセンサの
バイアス誤差推定をしている。
【００４９】実施の形態４．誤差分散行列の固有値算出
器に加えて、観測行列の階数減少により誤差推定値が発
生するのを防ぐペナルティ関数を用いて一度にバイアス
誤差を算出する装置を説明する。図６はこの発明の実施
の形態４のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９は従来の装置と、１０、１６〜１８、２
５〜２７は実施の形態１で用いられた要素とそれぞれ同
一である。２０は観測行列から、バイアス誤差推定値の
発散を抑えるペナルティ関数を用いてセンサのバイアス
誤差を算出するペナルティ関数型推定値算出器である。
器に加えて、観測行列の階数減少により誤差推定値が発
生するのを防ぐペナルティ関数を用いて一度にバイアス
誤差を算出する装置を説明する。図６はこの発明の実施
の形態４のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９は従来の装置と、１０、１６〜１８、２
５〜２７は実施の形態１で用いられた要素とそれぞれ同
一である。２０は観測行列から、バイアス誤差推定値の
発散を抑えるペナルティ関数を用いてセンサのバイアス
誤差を算出するペナルティ関数型推定値算出器である。
【００５０】次に原理について説明する。第１、及び、
第２のセンサから目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。また続いて、目標物の位置記憶器１６に記憶
された誤差分散行列の仮の値の最小固有値が式（３５）
を満たすときの固定目標物との相対位置データに対応す
る位置もしくは航跡にｋ個の各可動目標を移動もしくは
運動させる処理までは実施の形態１と同一である。次
に、ｋ個の可動目標の１つあるいは複数の配置位置の精
度が悪く、バイアス誤差推定結果を発散させる危険性の
ある観測行列が得られた場合にも、バイアス誤差の発散
を抑える評価関数を定義する。すなわち、従来の装置に
おける式（１３）の線形状態方程式に最小自乗法を適用
し、観測値と状態変数ｘアンダーバーとの差の自乗にバ
イアス誤差の推定精度を向上させる項を加えた以下の式
で表されるペナルティ関数を用いる。
第２のセンサから目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。また続いて、目標物の位置記憶器１６に記憶
された誤差分散行列の仮の値の最小固有値が式（３５）
を満たすときの固定目標物との相対位置データに対応す
る位置もしくは航跡にｋ個の各可動目標を移動もしくは
運動させる処理までは実施の形態１と同一である。次
に、ｋ個の可動目標の１つあるいは複数の配置位置の精
度が悪く、バイアス誤差推定結果を発散させる危険性の
ある観測行列が得られた場合にも、バイアス誤差の発散
を抑える評価関数を定義する。すなわち、従来の装置に
おける式（１３）の線形状態方程式に最小自乗法を適用
し、観測値と状態変数ｘアンダーバーとの差の自乗にバ
イアス誤差の推定精度を向上させる項を加えた以下の式
で表されるペナルティ関数を用いる。
【００５１】
【数１０】
【００５２】この式（３７）の右辺は行列Ｈt TＨt の最
小固有値が０に近くなった場合にもｘt アンダーバーハ
ットが大きくなり過ぎないようにするための項であり、
Ｌは対角行列で対角要素は小さな正の定数である。評価
関数Ｊt を最小にするｘt アンダーバーハットは、式
（３７）の右辺を展開して、ｘt アンダーバーハットに
ついて微分した結果が零になるようなｘt アンダーバー
ハットである。すなわち、次式（３８）を解き、バイア
ス誤差の推定結果である式（３９）を得る。
小固有値が０に近くなった場合にもｘt アンダーバーハ
ットが大きくなり過ぎないようにするための項であり、
Ｌは対角行列で対角要素は小さな正の定数である。評価
関数Ｊt を最小にするｘt アンダーバーハットは、式
（３７）の右辺を展開して、ｘt アンダーバーハットに
ついて微分した結果が零になるようなｘt アンダーバー
ハットである。すなわち、次式（３８）を解き、バイア
ス誤差の推定結果である式（３９）を得る。
【００５３】
【数１１】
【００５４】次に、上記で説明した原理を適用した実施
の形態４の装置の動作を図６、および、図７にしたがっ
て説明する。図６において、第１のセンサ１、及び、第
２のセンサ２から目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。また続いて、目標物の相対位置記憶器１６に
記憶された誤差分散行列の仮の値の最小固有値が式（３
５）を満たすときの固定目標物との相対位置データに対
応する位置もしくは航跡にｋ個の可動目標を移動もしく
は運動させる処理までは実施の形態１と同一である。次
に図７にしたがって、各可動目標が所定の位置に移動し
た後の、ペナルティ関数による推定値２０の処理の流れ
を説明する。ペナルティ関数による推定値算出器２０で
は、観測行列生成器９からの観測行列から式（３９）で
表されるバイアス誤差推定値を算出する。具体的には以
下のようになる。ステップ１００では観測行列生成器９
からの観測行列を取り込む。ステップ１０５では、初期
バイアス設定器８からのバイアス誤差の初期値を取り込
む。ステップ１０６では、ステップ１０５で取り込んだ
バイアス誤差の初期値と第１のセンサ１および第２のセ
ンサ２の位置ベクトルから、式（１４）〜式（１８）で
表される状態方程式の観測値を算出する。ステップ１０
７では、ステップ１０６で算出した状態方程式の観測値
と、観測ベクトルＨt と、あらかじめ設定された第１の
センサ１と第２のセンサ２の観測誤差の分散から、式
（３９）を用いてバイアス誤差の推定値を算出し、ステ
ップ１０８でバイアス誤差推定値１４を出力する。
の形態４の装置の動作を図６、および、図７にしたがっ
て説明する。図６において、第１のセンサ１、及び、第
２のセンサ２から目標の位置を観測し観測行列生成器９
で目標の観測行列を生成する処理までは従来の装置と同
一である。また続いて、目標物の相対位置記憶器１６に
記憶された誤差分散行列の仮の値の最小固有値が式（３
５）を満たすときの固定目標物との相対位置データに対
応する位置もしくは航跡にｋ個の可動目標を移動もしく
は運動させる処理までは実施の形態１と同一である。次
に図７にしたがって、各可動目標が所定の位置に移動し
た後の、ペナルティ関数による推定値２０の処理の流れ
を説明する。ペナルティ関数による推定値算出器２０で
は、観測行列生成器９からの観測行列から式（３９）で
表されるバイアス誤差推定値を算出する。具体的には以
下のようになる。ステップ１００では観測行列生成器９
からの観測行列を取り込む。ステップ１０５では、初期
バイアス設定器８からのバイアス誤差の初期値を取り込
む。ステップ１０６では、ステップ１０５で取り込んだ
バイアス誤差の初期値と第１のセンサ１および第２のセ
ンサ２の位置ベクトルから、式（１４）〜式（１８）で
表される状態方程式の観測値を算出する。ステップ１０
７では、ステップ１０６で算出した状態方程式の観測値
と、観測ベクトルＨt と、あらかじめ設定された第１の
センサ１と第２のセンサ２の観測誤差の分散から、式
（３９）を用いてバイアス誤差の推定値を算出し、ステ
ップ１０８でバイアス誤差推定値１４を出力する。
【００５５】このように、バイアス誤差の推定値が発散
する恐れのある観測行列に対しても、その発生を抑える
項を付加した評価関数を用いているので、信頼性が高く
精度の良いセンサのバイアス誤差推定値を得ることがで
きる。
する恐れのある観測行列に対しても、その発生を抑える
項を付加した評価関数を用いているので、信頼性が高く
精度の良いセンサのバイアス誤差推定値を得ることがで
きる。
【００５６】実施の形態５．実施の形態３と４を組み合
わせた場合を説明する。図８はこの発明の実施の形態５
のセンサのバイアス誤差推定装置の構成図である。図中
１〜９は従来の装置と、また、１０、１６〜１９、２５
〜２７は実施の形態３と、また、２０は実施の形態４で
用いられた要素とそれぞれ同一である。
わせた場合を説明する。図８はこの発明の実施の形態５
のセンサのバイアス誤差推定装置の構成図である。図中
１〜９は従来の装置と、また、１０、１６〜１９、２５
〜２７は実施の形態３と、また、２０は実施の形態４で
用いられた要素とそれぞれ同一である。
【００５７】次に、本実施の形態の装置の動作を図８に
従って説明する。第１、及び、第２のセンサから固定目
標との相対位置を観測し目標物の相対位置記憶器１６に
記憶された誤差分散行列の仮の値の最小固有値が式（３
５）を満たすときの固定目標物との相対位置データに対
応する位置もしくは航跡にｋ個の目標を移動もしくは運
動させる処理までは実施の形態３と同一である。その
後、実施の形態４と同様に、ペナルティ関数による推定
値算出器２０で、観測行列生成器９からの観測行列から
式（３９）で表されるバイアス誤差推定値を算出する。
このように、バイアス誤差の推定値が発散する恐れのあ
る観測行列に対しても、その発生を抑える項を付加した
評価関数を用いているので、信頼性が高く精度の良いセ
ンサのバイアス誤差推定値を得ることができる。実施の
形態２と実施の形態４を組合せてもよい。
従って説明する。第１、及び、第２のセンサから固定目
標との相対位置を観測し目標物の相対位置記憶器１６に
記憶された誤差分散行列の仮の値の最小固有値が式（３
５）を満たすときの固定目標物との相対位置データに対
応する位置もしくは航跡にｋ個の目標を移動もしくは運
動させる処理までは実施の形態３と同一である。その
後、実施の形態４と同様に、ペナルティ関数による推定
値算出器２０で、観測行列生成器９からの観測行列から
式（３９）で表されるバイアス誤差推定値を算出する。
このように、バイアス誤差の推定値が発散する恐れのあ
る観測行列に対しても、その発生を抑える項を付加した
評価関数を用いているので、信頼性が高く精度の良いセ
ンサのバイアス誤差推定値を得ることができる。実施の
形態２と実施の形態４を組合せてもよい。
【００５８】実施の形態６．ペナルティ関数による推定
値算出器に換え、観測行列と観測雑音の共分散とから同
じくペナルティ関数を用いて推定値の共分散行列を得
て、更に再帰的に誤差を算出する装置を説明する。図９
はこの発明の実施の形態５のセンサのバイアス誤差推定
装置の構成を示す。図中１〜９及び１３〜１５は従来の
装置と、同一である。１０、１６〜１８、２５〜２７は
実施の形態１で用いられた要素とそれぞれ同一である。
２１は観測行列から、バイアス誤差推定値の発散を抑え
るペナルティ関数を用いてバイアス誤差の推定値の共分
散行列を算出するペナルティ関数型推定値評価器、２２
はペナルティ関数型推定値評価器２１からのバイアス誤
差の推定値の共分散行列と、観測行列とから、バイアス
誤差推定値の発散を抑えるペナルティ関数を用いて再帰
的にセンサのバイアス誤差を算出するペナルティ関数型
再帰的推定値算出器である。
値算出器に換え、観測行列と観測雑音の共分散とから同
じくペナルティ関数を用いて推定値の共分散行列を得
て、更に再帰的に誤差を算出する装置を説明する。図９
はこの発明の実施の形態５のセンサのバイアス誤差推定
装置の構成を示す。図中１〜９及び１３〜１５は従来の
装置と、同一である。１０、１６〜１８、２５〜２７は
実施の形態１で用いられた要素とそれぞれ同一である。
２１は観測行列から、バイアス誤差推定値の発散を抑え
るペナルティ関数を用いてバイアス誤差の推定値の共分
散行列を算出するペナルティ関数型推定値評価器、２２
はペナルティ関数型推定値評価器２１からのバイアス誤
差の推定値の共分散行列と、観測行列とから、バイアス
誤差推定値の発散を抑えるペナルティ関数を用いて再帰
的にセンサのバイアス誤差を算出するペナルティ関数型
再帰的推定値算出器である。
【００５９】次に原理について説明する。第１のセンサ
１、および、第２のセンサ２から目標の位置を観測し観
測行列生成器９で目標の観測行列を生成する処理までは
従来の装置と同一である。また続いて、目標物の位置記
憶器１６に記憶された誤差分散行列の仮の値の最小固有
値が式（３５）を満たすときの固定目標物との相対位置
データに対応する位置もしくは航跡にｋ個の目標を移動
もしくは運動させる処理までは実施の形態１と同一であ
る。またそれ以後の、最初のサンプリング時の処理は実
施の形態４と同一である。このときの、推定値の共分散
行列は次式（４０）で与えられる。
１、および、第２のセンサ２から目標の位置を観測し観
測行列生成器９で目標の観測行列を生成する処理までは
従来の装置と同一である。また続いて、目標物の位置記
憶器１６に記憶された誤差分散行列の仮の値の最小固有
値が式（３５）を満たすときの固定目標物との相対位置
データに対応する位置もしくは航跡にｋ個の目標を移動
もしくは運動させる処理までは実施の形態１と同一であ
る。またそれ以後の、最初のサンプリング時の処理は実
施の形態４と同一である。このときの、推定値の共分散
行列は次式（４０）で与えられる。
【００６０】
【数１２】
【００６１】それ以後のサンプルでは、再帰的算出方法
を用いる。すなわち、従来の装置における式（１３）の
線形状態方程式に最小自乗法を適用し、観測値と状態変
数ｘアンダーバーとの差の自乗にバイアス誤差の推定精
度を向上させる項を加えた以下の式（４１）で表される
ペナルティ関数を用いる。ここで、式（４２）、（４
３）としている。
を用いる。すなわち、従来の装置における式（１３）の
線形状態方程式に最小自乗法を適用し、観測値と状態変
数ｘアンダーバーとの差の自乗にバイアス誤差の推定精
度を向上させる項を加えた以下の式（４１）で表される
ペナルティ関数を用いる。ここで、式（４２）、（４
３）としている。
【００６２】
【数１３】
【００６３】式（４０）の右辺は、行列Ｈt TＨt の最小
固有値が０に近くなった場合にもｘt アンダーバーハッ
トが大きくなり過ぎないようにするための項であり、Ｌ
は対角行列で対角要素は小さな正の定数である。評価関
数Ｊt を最小にするｘt アンダーバーハットは、式（３
９）の右辺を展開して、ｘt アンダーバーハットについ
て微分した結果が零になるようなｘt アンダーバーハッ
トである。すなわち、次式（４４）を解き、バイアス誤
差の推定結果である式（４５）ないし式（４７）を得
る。
固有値が０に近くなった場合にもｘt アンダーバーハッ
トが大きくなり過ぎないようにするための項であり、Ｌ
は対角行列で対角要素は小さな正の定数である。評価関
数Ｊt を最小にするｘt アンダーバーハットは、式（３
９）の右辺を展開して、ｘt アンダーバーハットについ
て微分した結果が零になるようなｘt アンダーバーハッ
トである。すなわち、次式（４４）を解き、バイアス誤
差の推定結果である式（４５）ないし式（４７）を得
る。
【００６４】
【数１４】
【００６５】次に、上記で説明した原理を適用した実施
の形態５の装置の動作を図８にしたがって説明する。第
１のセンサ１、及び、第２のセンサ２から目標の位置を
観測し観測行列生成器９で目標の観測行列を生成する処
理までは従来の装置と同一である。また続いて、目標物
の相対位置記憶器１６に記憶された誤差分散行列の仮の
値の最小固有値が式（３５）を満たすときの固定目標物
との相対位置データに対応する位置もしくは航跡にｋ個
の目標を移動もしくは運動させる処理までは実施の形態
１と同一である。ペナルティ関数型推定値評価器２１で
は、最初のサンプリング時には、観測行列生成器９から
の観測行列から式（４０）の推定値の共分散行列を演算
し、ペナルティ関数による再帰的推定値算出器１９に送
出する。それ以後のサンプルでは観測行列生成器９から
の観測行列から式（４６）即ち、推定値の共分散行列を
演算し、ペナルティ関数型再帰的推定値算出器２２に送
出する。
の形態５の装置の動作を図８にしたがって説明する。第
１のセンサ１、及び、第２のセンサ２から目標の位置を
観測し観測行列生成器９で目標の観測行列を生成する処
理までは従来の装置と同一である。また続いて、目標物
の相対位置記憶器１６に記憶された誤差分散行列の仮の
値の最小固有値が式（３５）を満たすときの固定目標物
との相対位置データに対応する位置もしくは航跡にｋ個
の目標を移動もしくは運動させる処理までは実施の形態
１と同一である。ペナルティ関数型推定値評価器２１で
は、最初のサンプリング時には、観測行列生成器９から
の観測行列から式（４０）の推定値の共分散行列を演算
し、ペナルティ関数による再帰的推定値算出器１９に送
出する。それ以後のサンプルでは観測行列生成器９から
の観測行列から式（４６）即ち、推定値の共分散行列を
演算し、ペナルティ関数型再帰的推定値算出器２２に送
出する。
【００６６】ペナルティ関数型再帰的推定値算出器２２
では、ペナルティ関数型推定値評価器２１からの共分散
行列と観測行列生成器９からの観測行列とから式（４
５）の右辺第２項を算出する。さらに、減算器１３でペ
ナルティ関数型再帰的推定値算出器２２からのバイアス
誤差推定値を初期バイアス設定器８からのバイアス誤差
初期値または前回算出のセンサのバイアス誤差推定値１
４から差し引いて式（２０）のサンプリング時刻ｔにお
けるバイアス誤差推定値１４を演算し出力するととも
に、推定値記憶器１５に送出する。推定値記憶器１５
は、算出したサンプリング時刻ｔにおけるセンサのバイ
アス誤差の推定値１４を記憶するとともに、前回算出の
センサのバイアス誤差推定値として初期バイアス誤差推
定器８に送出する。以後、この一連の処理をバイアス誤
差の推定値が収束するまで繰り返す。なお、このとき初
期バイアス誤差推定器８からはその都度前回（サンプリ
ング時刻（ｔ−１））算出のセンサのバイアス誤差推定
値が出力される。このように、バイアス誤差の推定値が
発散する恐れのある観測行列に対しても、その発生を抑
える項を付加した評価関数を用いているので、信頼性が
高く精度の良いセンサのバイアス誤差推定値を得ること
ができる。さらに、前回算出した推定値を使ってバイア
ス誤差を収束計算するのでより精度がよい。
では、ペナルティ関数型推定値評価器２１からの共分散
行列と観測行列生成器９からの観測行列とから式（４
５）の右辺第２項を算出する。さらに、減算器１３でペ
ナルティ関数型再帰的推定値算出器２２からのバイアス
誤差推定値を初期バイアス設定器８からのバイアス誤差
初期値または前回算出のセンサのバイアス誤差推定値１
４から差し引いて式（２０）のサンプリング時刻ｔにお
けるバイアス誤差推定値１４を演算し出力するととも
に、推定値記憶器１５に送出する。推定値記憶器１５
は、算出したサンプリング時刻ｔにおけるセンサのバイ
アス誤差の推定値１４を記憶するとともに、前回算出の
センサのバイアス誤差推定値として初期バイアス誤差推
定器８に送出する。以後、この一連の処理をバイアス誤
差の推定値が収束するまで繰り返す。なお、このとき初
期バイアス誤差推定器８からはその都度前回（サンプリ
ング時刻（ｔ−１））算出のセンサのバイアス誤差推定
値が出力される。このように、バイアス誤差の推定値が
発散する恐れのある観測行列に対しても、その発生を抑
える項を付加した評価関数を用いているので、信頼性が
高く精度の良いセンサのバイアス誤差推定値を得ること
ができる。さらに、前回算出した推定値を使ってバイア
ス誤差を収束計算するのでより精度がよい。
【００６７】実施の形態７．実施の形態３と６を組み合
わせた場合を説明する。上記の各実施の形態では、各可
動目標１ないしＫは静止物体として地上の固定目標物を
用いる場合を説明した。静止物体としてはこの他に、無
線航法施設からの信号によってもよい。更に、この静止
物体自体を複数個用いて相対位置を詳しく定めるように
してもよい。図１０はこの発明の実施の形態７のセンサ
のバイアス誤差推定装置の構成図である。図中１〜９及
び１３〜１５は従来の装置と、また、１０、１６〜１
９、２５〜２７は実施の形態３と、また、１３〜１５、
２１〜２２は実施の形態６で用いられた要素とそれぞれ
同一である。
わせた場合を説明する。上記の各実施の形態では、各可
動目標１ないしＫは静止物体として地上の固定目標物を
用いる場合を説明した。静止物体としてはこの他に、無
線航法施設からの信号によってもよい。更に、この静止
物体自体を複数個用いて相対位置を詳しく定めるように
してもよい。図１０はこの発明の実施の形態７のセンサ
のバイアス誤差推定装置の構成図である。図中１〜９及
び１３〜１５は従来の装置と、また、１０、１６〜１
９、２５〜２７は実施の形態３と、また、１３〜１５、
２１〜２２は実施の形態６で用いられた要素とそれぞれ
同一である。
【００６８】次に、本実施の形態の装置の動作を図１０
に従って説明する。第１、及び、第２のセンサから可動
目標の位置を観測し目標物の相対位置記憶器１６に記憶
された誤差分散行列の仮の値の最小固有値及びＨt TＨt
の最小固有値がそれぞれ式（３５）及び式を（３６）を
同時に満たすときの固定目標物との相対位置データに対
応する位置もしくは航跡にｋ個の目標を移動もしくは運
動させる処理までは実施の形態３と同一である。その
後、ペナルティ関数型推定値評価器２１では、最初のサ
ンプリング時には、観測行列生成器９からの観測行列か
ら式（４０）の推定値の共分散行列を演算し、ペナルテ
ィ関数による再帰的推定値算出器１９に送出する。それ
以後のサンプルでは観測行列生成器９からの観測行列か
ら式（４６）即ち、推定値の共分散行列を演算し、ペナ
ルティ関数型再帰的推定値算出器２２に送出する。ペナ
ルティ関数型再帰的推定値算出器２２以降の動作は実施
の形態６での動作と同じである。実施の形態２と実施の
形態６を組合わせてもよい。
に従って説明する。第１、及び、第２のセンサから可動
目標の位置を観測し目標物の相対位置記憶器１６に記憶
された誤差分散行列の仮の値の最小固有値及びＨt TＨt
の最小固有値がそれぞれ式（３５）及び式を（３６）を
同時に満たすときの固定目標物との相対位置データに対
応する位置もしくは航跡にｋ個の目標を移動もしくは運
動させる処理までは実施の形態３と同一である。その
後、ペナルティ関数型推定値評価器２１では、最初のサ
ンプリング時には、観測行列生成器９からの観測行列か
ら式（４０）の推定値の共分散行列を演算し、ペナルテ
ィ関数による再帰的推定値算出器１９に送出する。それ
以後のサンプルでは観測行列生成器９からの観測行列か
ら式（４６）即ち、推定値の共分散行列を演算し、ペナ
ルティ関数型再帰的推定値算出器２２に送出する。ペナ
ルティ関数型再帰的推定値算出器２２以降の動作は実施
の形態６での動作と同じである。実施の形態２と実施の
形態６を組合わせてもよい。
【００６９】実施の形態８．図１１はこの発明の実施の
形態８のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９及び１１〜１５は従来の装置と、また、
１０、２３、２４、２６ａ、２６ｂ、２６ｃ、２７は実
施の形態１で用いられた要素とそれぞれ同一である。２
８ａ、２８ｂ、２８ｃはｋ個の目標それぞれの位置を計
測するＧＰＳ（Global Positioning System）装置であ
る。
形態８のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９及び１１〜１５は従来の装置と、また、
１０、２３、２４、２６ａ、２６ｂ、２６ｃ、２７は実
施の形態１で用いられた要素とそれぞれ同一である。２
８ａ、２８ｂ、２８ｃはｋ個の目標それぞれの位置を計
測するＧＰＳ（Global Positioning System）装置であ
る。
【００７０】次に、本実施の形態の装置の動作を図１１
に従って説明する。図１１において、第１のセンサ１、
及び、第２のセンサ２から目標の位置を観測し観測行列
生成器９で目標の観測行列を生成する処理までは従来の
装置と同一である。誤差分散行列の固有値算出器１０で
は、観測行列の誤差分散行列の仮の値の最小固有値を算
出する。ｋ個の目標それぞれに搭載しているＧＰＳ装置
２８では、衛星からの電波の伝搬時間より距離を測定
し、ｋ個の目標各々の位置を決定する。このＧＰＳ装置
２８により計測した各可動目標の絶対位置データを第２
の通信装置２７で受けて、目標の絶対位置記憶器２３に
出力する。目標の絶対位置記憶器２３では誤差分散行列
の固有値算出器１０で算出した観測行列の誤差分散行列
の仮の値の最小固有値と、上記の各可動目標の絶対位置
データとをサンプル時刻毎に記憶する。目標の絶対位置
評価器２４では目標の絶対位置記憶器２３に記憶された
最小固有値が式（３５）を満たすときの目標の位置デー
タを検索して出力する。ここで、ある定数ａの値は計算
機の精度やセンサの精度により決まる値である。目標位
置指示器１８では目標の絶対位置評価器２４で検索した
可動目標の絶対位置データを第２の通信装置２７を介し
て送出する。ｋ個の可動目標は、この送出された各可動
目標の絶対位置データを受信し、各目標の操縦者はＧＰ
Ｓ装置２８ａ、２８ｂ、２８ｃで自機の位置を計算しな
がら、それが通信装置２６ａ、２６ｂ、２６ｃで受信し
た各可動目標の絶対位置データと等しくなる位置もしく
は航跡に各目標を移動もしくは運動させる。その後は、
従来の装置と同様に推定値評価器１１以降の処理を行
い、第１のセンサ１および第２のセンサ２のバイアス誤
差の推定値を算出する。
に従って説明する。図１１において、第１のセンサ１、
及び、第２のセンサ２から目標の位置を観測し観測行列
生成器９で目標の観測行列を生成する処理までは従来の
装置と同一である。誤差分散行列の固有値算出器１０で
は、観測行列の誤差分散行列の仮の値の最小固有値を算
出する。ｋ個の目標それぞれに搭載しているＧＰＳ装置
２８では、衛星からの電波の伝搬時間より距離を測定
し、ｋ個の目標各々の位置を決定する。このＧＰＳ装置
２８により計測した各可動目標の絶対位置データを第２
の通信装置２７で受けて、目標の絶対位置記憶器２３に
出力する。目標の絶対位置記憶器２３では誤差分散行列
の固有値算出器１０で算出した観測行列の誤差分散行列
の仮の値の最小固有値と、上記の各可動目標の絶対位置
データとをサンプル時刻毎に記憶する。目標の絶対位置
評価器２４では目標の絶対位置記憶器２３に記憶された
最小固有値が式（３５）を満たすときの目標の位置デー
タを検索して出力する。ここで、ある定数ａの値は計算
機の精度やセンサの精度により決まる値である。目標位
置指示器１８では目標の絶対位置評価器２４で検索した
可動目標の絶対位置データを第２の通信装置２７を介し
て送出する。ｋ個の可動目標は、この送出された各可動
目標の絶対位置データを受信し、各目標の操縦者はＧＰ
Ｓ装置２８ａ、２８ｂ、２８ｃで自機の位置を計算しな
がら、それが通信装置２６ａ、２６ｂ、２６ｃで受信し
た各可動目標の絶対位置データと等しくなる位置もしく
は航跡に各目標を移動もしくは運動させる。その後は、
従来の装置と同様に推定値評価器１１以降の処理を行
い、第１のセンサ１および第２のセンサ２のバイアス誤
差の推定値を算出する。
【００７１】このように、目標とセンサの幾何学的な位
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、誤差分散行列の正則が保たれるようにｋ個の
目標を配置した上でバイアス誤差の推定演算をしている
ので、移動後の各可動目標による第１と第２のセンサに
よる高精度のセンサのバイアス誤差推定値を得ることが
できる。
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、誤差分散行列の正則が保たれるようにｋ個の
目標を配置した上でバイアス誤差の推定演算をしている
ので、移動後の各可動目標による第１と第２のセンサに
よる高精度のセンサのバイアス誤差推定値を得ることが
できる。
【００７２】実施の形態９． 誤差分散行列の固有値算出器に換えて、観測行列の階数
を評価してｋ個の目標を配置させる装置を説明する。図
１２はこの発明の実施の形態９を示すセンサのバイアス
誤差推定装置の構成図である。図中１〜９及び１１〜１
５は従来の装置と同一である。また、１８、２３、２
４、２６〜２８は実施の形態８と同一である。１９はサ
ンプリング時刻ｔにおける観測行列の階数による解析か
ら得た判定式を算出する階数評価値算出器である。
を評価してｋ個の目標を配置させる装置を説明する。図
１２はこの発明の実施の形態９を示すセンサのバイアス
誤差推定装置の構成図である。図中１〜９及び１１〜１
５は従来の装置と同一である。また、１８、２３、２
４、２６〜２８は実施の形態８と同一である。１９はサ
ンプリング時刻ｔにおける観測行列の階数による解析か
ら得た判定式を算出する階数評価値算出器である。
【００７３】次に、本実施の形態の装置の動作を図１２
に従って説明する。図１２において、第１のセンサ１、
及び、第２のセンサ２から目標の位置を観測し観測行列
生成器９で目標の観測行列を生成する処理までは従来の
装置と同一である。階数評価値算出器１９では、Ｈt TＨ
t の最小固有値を算出する。これは、実施の形態２と同
様であるので詳細説明は省略する。ＧＰＳ装置２８ａ、
２８ｂ、２８ｃでｋ個の可動目標各々の位置を計算する
処理から、各可動目標の絶対位置データを、目標の絶対
位置記憶器２３に記憶するまでの処理は実施の形態８と
同一である。目標の絶対位置記憶器２３では階数評価値
算出器１９で算出したＨt TＨt の最小固有値と、上記各
可動目標の絶対位置データとをサンプル時刻毎に記憶す
る。目標の絶対位置評価器２４では、目標の絶対位置記
憶器２３に記憶された最小固有値が式（３６）を満たす
ときの各可動目標の絶対位置データを検索して出力す
る。目標位置指示器１８が各可動目標の絶対位置データ
を送出し、各可動目標の操縦者が位置もしくは航跡に移
動もしくは運動させる動作も実施の形態８と同様であ
る。その後は、従来の装置と同様に推定値評価器１１以
降の処理を行い、第１のセンサ１および第２のセンサ２
のバイアス誤差の推定値を算出する。
に従って説明する。図１２において、第１のセンサ１、
及び、第２のセンサ２から目標の位置を観測し観測行列
生成器９で目標の観測行列を生成する処理までは従来の
装置と同一である。階数評価値算出器１９では、Ｈt TＨ
t の最小固有値を算出する。これは、実施の形態２と同
様であるので詳細説明は省略する。ＧＰＳ装置２８ａ、
２８ｂ、２８ｃでｋ個の可動目標各々の位置を計算する
処理から、各可動目標の絶対位置データを、目標の絶対
位置記憶器２３に記憶するまでの処理は実施の形態８と
同一である。目標の絶対位置記憶器２３では階数評価値
算出器１９で算出したＨt TＨt の最小固有値と、上記各
可動目標の絶対位置データとをサンプル時刻毎に記憶す
る。目標の絶対位置評価器２４では、目標の絶対位置記
憶器２３に記憶された最小固有値が式（３６）を満たす
ときの各可動目標の絶対位置データを検索して出力す
る。目標位置指示器１８が各可動目標の絶対位置データ
を送出し、各可動目標の操縦者が位置もしくは航跡に移
動もしくは運動させる動作も実施の形態８と同様であ
る。その後は、従来の装置と同様に推定値評価器１１以
降の処理を行い、第１のセンサ１および第２のセンサ２
のバイアス誤差の推定値を算出する。
【００７４】このように、観測行列の階数が５以下の状
況下でも、目標の観測位置等の若干の違いによりその階
数が６となってセンサのバイアス誤差推定値が真値とか
け離れてしまう現象を回避するように、観測行列の階数
を評価してｋ個の目標を配置した上でバイアス誤差の推
定演算をしているので、精度の良いセンサのバイアス誤
差推定値を得ることができる。
況下でも、目標の観測位置等の若干の違いによりその階
数が６となってセンサのバイアス誤差推定値が真値とか
け離れてしまう現象を回避するように、観測行列の階数
を評価してｋ個の目標を配置した上でバイアス誤差の推
定演算をしているので、精度の良いセンサのバイアス誤
差推定値を得ることができる。
【００７５】実施の形態１０．実施の形態８と９を組み
合わせた場合を説明する。図１３はこの発明の実施の形
態１０のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９及び１１〜１５は従来の装置と、また、
１０、１８、２３、２４、２６〜２８は実施の形態８
で、１９は実施の形態９で用いられた要素とそれぞれ同
一である。
合わせた場合を説明する。図１３はこの発明の実施の形
態１０のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９及び１１〜１５は従来の装置と、また、
１０、１８、２３、２４、２６〜２８は実施の形態８
で、１９は実施の形態９で用いられた要素とそれぞれ同
一である。
【００７６】次に、本実施の形態の装置の動作を図１３
に従って説明する。第１、及び、第２のセンサから目標
の位置を観測し誤差分散行列の固有値算出器１０で行列
Ａt の最小固有値を算出する処理までは実施の形態８と
同一である。階数評価値算出器１９は、実施の形態９と
同様の動作をする。目標の絶対位置記憶器２３では誤差
分散行列の固有値算出器１０で算出した観測行列の誤差
分散行列の仮の値の最小固有値と、階数評価値算出器１
９で算出したＨt TＨt の最小固有値と、第２の通信装置
２７で受信した各可動目標の絶対位置データとをサンプ
ル時刻毎に記憶する。目標の絶対位置評価器２４では目
標の絶対位置記憶器２３に記憶された誤差分散行列の仮
の値の最小固有値及びＨt TＨt の最小の最小固有値がそ
れぞれ式（３５）及び式を（３６）を同時に満たすとき
の各可動目標の絶対位置データを検索して出力する。目
標位置指示器１８が各可動目標の絶対位置データを送出
し、対応して各可動目標の操縦者が送出された位置もし
くは航跡に各目標を移動もしくは運動させる動作も実施
の形態８と同様である。その後は、従来の装置と同様に
推定値評価器１１以降の処理を行い、第１のセンサ１お
よび第２のセンサ２のバイアス誤差の推定値を算出す
る。
に従って説明する。第１、及び、第２のセンサから目標
の位置を観測し誤差分散行列の固有値算出器１０で行列
Ａt の最小固有値を算出する処理までは実施の形態８と
同一である。階数評価値算出器１９は、実施の形態９と
同様の動作をする。目標の絶対位置記憶器２３では誤差
分散行列の固有値算出器１０で算出した観測行列の誤差
分散行列の仮の値の最小固有値と、階数評価値算出器１
９で算出したＨt TＨt の最小固有値と、第２の通信装置
２７で受信した各可動目標の絶対位置データとをサンプ
ル時刻毎に記憶する。目標の絶対位置評価器２４では目
標の絶対位置記憶器２３に記憶された誤差分散行列の仮
の値の最小固有値及びＨt TＨt の最小の最小固有値がそ
れぞれ式（３５）及び式を（３６）を同時に満たすとき
の各可動目標の絶対位置データを検索して出力する。目
標位置指示器１８が各可動目標の絶対位置データを送出
し、対応して各可動目標の操縦者が送出された位置もし
くは航跡に各目標を移動もしくは運動させる動作も実施
の形態８と同様である。その後は、従来の装置と同様に
推定値評価器１１以降の処理を行い、第１のセンサ１お
よび第２のセンサ２のバイアス誤差の推定値を算出す
る。
【００７７】このように、目標とセンサの幾何学的な位
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、また、観測行列の階数が５以下の状況下で
も、目標の観測位置等の若干の違いによりその階数が６
となってセンサのバイアス誤差推定値が真値とかけ離れ
てしまう現象を回避するように、誤差分散行列の正則性
及び観測行列の階数を評価してｋ個の目標を配置した上
でバイアス誤差の推定演算をしているので、精度の良い
センサのバイアス誤差推定値を得ることができる。
置関係により発生する計算機丸め誤差の蓄積が起こらな
いように、また、観測行列の階数が５以下の状況下で
も、目標の観測位置等の若干の違いによりその階数が６
となってセンサのバイアス誤差推定値が真値とかけ離れ
てしまう現象を回避するように、誤差分散行列の正則性
及び観測行列の階数を評価してｋ個の目標を配置した上
でバイアス誤差の推定演算をしているので、精度の良い
センサのバイアス誤差推定値を得ることができる。
【００７８】実施の形態１１．実施の形態８と４を組み
合わせた場合を説明する。図１４はこの発明の実施の形
態１１のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９は従来の装置と、１０、１８、２３、２
４、２６〜２８は実施の形態８で、２０は実施の形態４
で用いられた要素とそれぞれ同一である。
合わせた場合を説明する。図１４はこの発明の実施の形
態１１のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９は従来の装置と、１０、１８、２３、２
４、２６〜２８は実施の形態８で、２０は実施の形態４
で用いられた要素とそれぞれ同一である。
【００７９】次に、本実施の形態の装置の動作を図１４
にしたがって説明する。図１４において、第１のセンサ
１、及び、第２のセンサ２から目標の絶対位置を観測し
観測行列生成器９で目標の観測行列を生成する処理まで
は従来の装置と同一である。また続いて、目標の絶対位
置記憶器２３に記憶された誤差分散行列の仮の値の最小
固有値が式（３５）を満たすときの各可動目標の絶対位
置データに対応する位置もしくは航跡にｋ個の目標を移
動もしくは運動させる処理までは実施の形態８と同一で
ある。その後のペナルティ関数による推定値算出器２０
で式（３９）を用いてバイアス誤差の推定値を算出する
処理は実施の形態４と同一である。
にしたがって説明する。図１４において、第１のセンサ
１、及び、第２のセンサ２から目標の絶対位置を観測し
観測行列生成器９で目標の観測行列を生成する処理まで
は従来の装置と同一である。また続いて、目標の絶対位
置記憶器２３に記憶された誤差分散行列の仮の値の最小
固有値が式（３５）を満たすときの各可動目標の絶対位
置データに対応する位置もしくは航跡にｋ個の目標を移
動もしくは運動させる処理までは実施の形態８と同一で
ある。その後のペナルティ関数による推定値算出器２０
で式（３９）を用いてバイアス誤差の推定値を算出する
処理は実施の形態４と同一である。
【００８０】このように、バイアス誤差の推定値が発散
する恐れのある観測行列に対しても、その発生を抑える
項を付加した評価関数を用いているので、信頼性が高く
精度の良いセンサのバイアス誤差推定値を得ることがで
きる。
する恐れのある観測行列に対しても、その発生を抑える
項を付加した評価関数を用いているので、信頼性が高く
精度の良いセンサのバイアス誤差推定値を得ることがで
きる。
【００８１】実施の形態１２．実施の形態１０と４を組
み合わせた場合を説明する。図１５はこの発明の実施の
形態１２のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９は従来の装置と、また、１０、１８、１
９、２３、２４、２６〜２８は実施の形態１０と、ま
た、２０は実施の形態４で用いられた要素とそれぞれ同
一である。本実施の形態の装置の動作は、実施の形態１
０と実施の形態４で説明したので詳細記述を省略する。
み合わせた場合を説明する。図１５はこの発明の実施の
形態１２のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９は従来の装置と、また、１０、１８、１
９、２３、２４、２６〜２８は実施の形態１０と、ま
た、２０は実施の形態４で用いられた要素とそれぞれ同
一である。本実施の形態の装置の動作は、実施の形態１
０と実施の形態４で説明したので詳細記述を省略する。
【００８２】実施の形態１３．実施の形態８と６を組み
合わせた場合を説明する。図１６はこの発明の実施の形
態１３のセンサのバイアス誤差推定装置の構成を示す。
図中１〜９及び１３〜１５は従来の装置と同一である。
１０、１８、２３、２４、２６〜２８は実施の形態８で
用いられた要素と、２１、２２は実施の形態６で用いら
れた要素とそれぞれ同一である。本実施の形態の構成の
装置についても、動作は上記各実施の形態で説明されて
いるので、詳細を再び記述することは冗長になるので省
略する。
合わせた場合を説明する。図１６はこの発明の実施の形
態１３のセンサのバイアス誤差推定装置の構成を示す。
図中１〜９及び１３〜１５は従来の装置と同一である。
１０、１８、２３、２４、２６〜２８は実施の形態８で
用いられた要素と、２１、２２は実施の形態６で用いら
れた要素とそれぞれ同一である。本実施の形態の構成の
装置についても、動作は上記各実施の形態で説明されて
いるので、詳細を再び記述することは冗長になるので省
略する。
【００８３】実施の形態１４．実施の形態１０と６を組
み合わせた場合を説明する。図１７はこの発明の実施の
形態１４のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９及び１３〜１５は従来の装置と、また、
１０、１８、１９、２３、２４、２６〜２８は実施の形
態１０と、また、２１〜２２は実施の形態６で用いられ
た要素とそれぞれ同一である。
み合わせた場合を説明する。図１７はこの発明の実施の
形態１４のセンサのバイアス誤差推定装置の構成図であ
る。図中１〜９及び１３〜１５は従来の装置と、また、
１０、１８、１９、２３、２４、２６〜２８は実施の形
態１０と、また、２１〜２２は実施の形態６で用いられ
た要素とそれぞれ同一である。
【００８４】本実施の形態の装置の動作は、実施の形態
１０と実施の形態６で説明したので詳細記述を省略す
る。
１０と実施の形態６で説明したので詳細記述を省略す
る。
【００８５】上記の実施の形態では、各可動目標１ない
しＫはＧＰＳにより自己位置のデータを得る場合を説明
した。絶対値として自己位置データを得るものとして、
他にも複数の静止衛星からの信号を識別してその組み合
わせから自己位置を得るようにしてもよい。
しＫはＧＰＳにより自己位置のデータを得る場合を説明
した。絶対値として自己位置データを得るものとして、
他にも複数の静止衛星からの信号を識別してその組み合
わせから自己位置を得るようにしてもよい。
【００８６】実施の形態１５．実施の形態８以降のセン
サのバイアス誤差推定装置は、自己位置データをＧＰＳ
装置により計測する場合を示している。自己位置データ
として絶対位置データを得る装置としては、以下に示す
ような初期座標を記憶した搭載ジャイロにより計測する
こともできる。図１８ないし図２４は、本実施の形態に
おける各種のセンサのバイアス誤差推定装置の構成図で
ある。図において新規な要素は、２９ａ、２９ｂ、２９
ｃで示されるジャイロによる自己位置算出装置である。
これらは予め初期座標値を設定しておいて以後の座標値
をジャイロにより得ることができる。従って先の実施の
形態におけるＧＰＳ装置による自己位置データを得るこ
とと同様に、各可動目標に置いて自己位置を知ることが
でき、第２の通信装置２７経由で絶対位置を送信し、か
つ目標位置指示器１８からの指示により所定の位置に移
動する。図１８ないし図２４の構成は、それぞれＧＰＳ
装置搭載の図１１ないし図１７に対応しているので、動
作の詳細記述は省略する。
サのバイアス誤差推定装置は、自己位置データをＧＰＳ
装置により計測する場合を示している。自己位置データ
として絶対位置データを得る装置としては、以下に示す
ような初期座標を記憶した搭載ジャイロにより計測する
こともできる。図１８ないし図２４は、本実施の形態に
おける各種のセンサのバイアス誤差推定装置の構成図で
ある。図において新規な要素は、２９ａ、２９ｂ、２９
ｃで示されるジャイロによる自己位置算出装置である。
これらは予め初期座標値を設定しておいて以後の座標値
をジャイロにより得ることができる。従って先の実施の
形態におけるＧＰＳ装置による自己位置データを得るこ
とと同様に、各可動目標に置いて自己位置を知ることが
でき、第２の通信装置２７経由で絶対位置を送信し、か
つ目標位置指示器１８からの指示により所定の位置に移
動する。図１８ないし図２４の構成は、それぞれＧＰＳ
装置搭載の図１１ないし図１７に対応しているので、動
作の詳細記述は省略する。
【００８７】
【発明の効果】以上のようにこの発明によれば、ｋ個の
目標を誤差分散行列の正則が保たれるように配置した上
でバイアス誤差の推定演算をしているので、目標とセン
サの幾何学的な位置関係に起因する計算機丸め誤差の蓄
積を抑え、精度良くセンサのバイアス誤差を推定できる
効果がある。
目標を誤差分散行列の正則が保たれるように配置した上
でバイアス誤差の推定演算をしているので、目標とセン
サの幾何学的な位置関係に起因する計算機丸め誤差の蓄
積を抑え、精度良くセンサのバイアス誤差を推定できる
効果がある。
【００８８】また、ｋ個の目標を、観測行列の階数が減
少しないように観測行列の各要素に連続な関数を判断基
準として配置した上でバイアス誤差の推定演算をしてい
るので、目標とセンサの幾何学的な位置関係に起因する
階数のランク落ちを抑え、精度良くセンサのバイアス誤
差を推定できる効果がある。
少しないように観測行列の各要素に連続な関数を判断基
準として配置した上でバイアス誤差の推定演算をしてい
るので、目標とセンサの幾何学的な位置関係に起因する
階数のランク落ちを抑え、精度良くセンサのバイアス誤
差を推定できる効果がある。
【００８９】また、ｋ個の目標の１つあるいは複数の配
置位置の精度が悪く、バイアス誤差推定結果を発散させ
る危険性のある観測行列が得られた場合にも、バイアス
誤差の発散を抑えるペナルティ関数を導入したので、目
標とセンサの幾何学的な影響に起因する計算機丸め誤差
の蓄積と、観測行列のランク落ちによるバイアス誤差の
推定値の発散を抑え、精度の良いセンサのバイアス誤差
推定値が得られる効果がある。さらに、再帰的推定値算
出器を用いてバイアス誤差推定値を収束演算する構成で
は、さらに精度が良くなる効果がある。
置位置の精度が悪く、バイアス誤差推定結果を発散させ
る危険性のある観測行列が得られた場合にも、バイアス
誤差の発散を抑えるペナルティ関数を導入したので、目
標とセンサの幾何学的な影響に起因する計算機丸め誤差
の蓄積と、観測行列のランク落ちによるバイアス誤差の
推定値の発散を抑え、精度の良いセンサのバイアス誤差
推定値が得られる効果がある。さらに、再帰的推定値算
出器を用いてバイアス誤差推定値を収束演算する構成で
は、さらに精度が良くなる効果がある。
【００９０】各特徴のある要素を組み合わせた構成によ
れば、上記単独の要素による特徴が組み合わされて、さ
らに精度の良い結果が得られる効果がある。
れば、上記単独の要素による特徴が組み合わされて、さ
らに精度の良い結果が得られる効果がある。
【図１】 この発明の実施の形態１におけるセンサのバ
イアス誤差推定装置の構成図である。
イアス誤差推定装置の構成図である。
【図２】 この発明の実施の形態１における誤差分散行
列の固有値算出器の動作フローチャート図である。
列の固有値算出器の動作フローチャート図である。
【図３】 この発明の実施の形態２におけるセンサのバ
イアス誤差推定装置の構成図である。
イアス誤差推定装置の構成図である。
【図４】 この発明の実施の形態２における階数評価値
算出器のフローチャート図である。
算出器のフローチャート図である。
【図５】 この発明の実施の形態３におけるセンサのバ
イアス誤差推定装置の構成図である。
イアス誤差推定装置の構成図である。
【図６】 この発明の実施の形態４におけるセンサのバ
イアス誤差推定装置の構成図である。
イアス誤差推定装置の構成図である。
【図７】 この発明の実施の形態４におけるペナルティ
関数による推定値算出器の動作フローチャート図であ
る。
関数による推定値算出器の動作フローチャート図であ
る。
【図８】 この発明の実施の形態５におけるセンサのバ
イアス誤差推定装置の構成図である。
イアス誤差推定装置の構成図である。
【図９】 この発明の実施の形態６におけるセンサのバ
イアス誤差推定装置の構成図である。
イアス誤差推定装置の構成図である。
【図１０】 この発明の実施の形態７におけるセンサの
バイアス誤差推定装置の構成図である。
バイアス誤差推定装置の構成図である。
【図１１】 この発明の実施の形態８におけるセンサの
バイアス誤差推定装置の構成図である。
バイアス誤差推定装置の構成図である。
【図１２】 この発明の実施の形態９におけるセンサの
バイアス誤差推定装置の構成図である。
バイアス誤差推定装置の構成図である。
【図１３】 この発明の実施の形態１０におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図１４】 この発明の実施の形態１１におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図１５】 この発明の実施の形態１２におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図１６】 この発明の実施の形態１３におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図１７】 この発明の実施の形態１４におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図１８】 この発明の実施の形態１５におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図１９】 この発明の実施の形態１５におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図２０】 この発明の実施の形態１５におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図２１】 この発明の実施の形態１５におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図２２】 この発明の実施の形態１５におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図２３】 この発明の実施の形態１５におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図２４】 この発明の実施の形態１５におけるセンサ
のバイアス誤差推定装置の構成図である。
のバイアス誤差推定装置の構成図である。
【図２５】 従来のセンサのバイアス誤差推定装置の構
成図である。
成図である。
１ 第１のセンサ、２ 第２のセンサ、３ 第１の観測
器、４ 第２の観測器、５ センサ位置設定器、６ 第
１の加算器、７ 第２の加算器、８ 初期バイアス設定
器、９ 観測行列生成器、１０ 誤差分散行列の固有値
算出器、１１推定値評価器、１２ 推定値算出器、１３
減算器、１４ センサのバイアス誤差推定値、１５
推定値記憶器、１６ 目標物の相対位置記憶器、１７
目標物の位置評価器、１８ 目標位置指示器、１９ 階
数評価値算出器、２０ ペナルティ関数による推定値算
出器、２１ ペナルティ関数型推定値評価器、２２ ペ
ナルティ関数型再帰的推定値算出器、２３ 目標の絶対
位置記憶器、２４ 目標の絶対位置評価器、２５ａ，２
５ｂ，２５ｃ 位置計測装置、２６ａ，２６ｂ，２６ｃ
第１の通信装置、２７ 第２の通信装置、２８ａ，２
８ｂ，２８ｃ ＧＰＳ装置、２９ａ，２９ｂ，２９ｃ
ジャイロによる自己位置算出装置。
器、４ 第２の観測器、５ センサ位置設定器、６ 第
１の加算器、７ 第２の加算器、８ 初期バイアス設定
器、９ 観測行列生成器、１０ 誤差分散行列の固有値
算出器、１１推定値評価器、１２ 推定値算出器、１３
減算器、１４ センサのバイアス誤差推定値、１５
推定値記憶器、１６ 目標物の相対位置記憶器、１７
目標物の位置評価器、１８ 目標位置指示器、１９ 階
数評価値算出器、２０ ペナルティ関数による推定値算
出器、２１ ペナルティ関数型推定値評価器、２２ ペ
ナルティ関数型再帰的推定値算出器、２３ 目標の絶対
位置記憶器、２４ 目標の絶対位置評価器、２５ａ，２
５ｂ，２５ｃ 位置計測装置、２６ａ，２６ｂ，２６ｃ
第１の通信装置、２７ 第２の通信装置、２８ａ，２
８ｂ，２８ｃ ＧＰＳ装置、２９ａ，２９ｂ，２９ｃ
ジャイロによる自己位置算出装置。
フロントページの続き (56)参考文献 特開 平６−94823（ＪＰ，Ａ) 特開 平７−306251（ＪＰ，Ａ) Ａ．Ｋｏｎｒａｄ，”Ｔｈｅ ｚｅｒ ｏ ｄｉｖｅｒｇｅｎｃｅ ｃｏｎｓｔ ｒａｉｎｔ ｉｎ ３Ｄ ｖｅｃｔｏｒ ｆｉｎｉｔｅ ｅｌｅｍｅｎｔ ｍｅ ｔｈｏｄｓ．”，Ｖｏｌ．９，Ｓｕｐｐ ｌｅｍｅｎｔ Ａ，ＣＯＭＰＥＬ，1990 （米），ｐｐ．７−16 Ｇ．−Ｗ．Ｙｅ他，”Ｆｉｎｉｔｅ ｅｌｅｍｅｎｔ ａｎａｌｙｓｉｓ ｏ ｆ ｅｌｅｃｔｒｏｍａｇｎｅｔｉｃ ｆｉｅｌｄ ｆｏｒ ｍｕｌｔｉｄｉｍ ｅｎｓｉｏｎａｌ ｒｆ ｃａｖｉｔｉ ｅｓ．”，Ｖｏｌ．60，Ｎｏ．７，Ｒｅ ｖｉｅｗ ｏｆ Ｓｃｉｅｎｔｉｆｉｃ Ｉｎｓｔｒｕｍｅｎｔｓ，Ａｍｅｒｉ ｃａｎ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｐ ｈｙｓｉｃｓ，1989（米），ｐｐ．1740 −1743 Ｆｒｅｄｒｉｃ Ｍ．Ｈａｍ他，”Ｏ ｂｓｅｒｖａｂｉｌｉｔｙ，Ｅｉｇｅｎ ｖａｌｕｅｓ，ａｎｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒｉｎｇ”，Ｖｏｌ．19，Ｎ ｏ．２，ＩＥＥＥ Ｔｒａｎｓａｃｔｉ ｏｎｓ ｏｎ Ａｅｒｏｓｐａｃｅ ａ ｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔ ｅｍｓ，ＩＥＥＥ，1983（米），ｐｐ. 269−273 大矢雅則 他、数理情報科学事典、 （株）朝倉書店、1995年11月10日、ｐ. 791−793 (58)調査した分野(Int.Cl.7，ＤＢ名) G01S 5/00 - 5/14 G01S 7/00 - 7/42 G01S 13/00 - 13/95 G01C 21/00 - 21/36 G01C 23/00 - 25/00 ＪＩＣＳＴファイル（ＪＯＩＳ)
Claims (9)
- 【請求項１】 第１のセンサによる目標からのデータ入
力により観測する第１の観測器と、上記第１のセンサと
は離れた位置にある第２のセンサからのデータ入力によ
り観測する第２の観測器とからの各時間毎の出力で観測
行列を得る観測行列生成器と、 上記観測行列の誤差分散行列の仮の値の固有値を算出す
る誤差分散行列の固有値算出器と、 上記観測行列の誤差分散行列の仮の値の最小固有値と、
ｋ個の可動目標に搭載された位置計測手段からの固定目
標との相対位置データとを、対応させて時系列で記憶す
る目標物の相対位置記憶器と、 上記目標物の相対位置記憶器で記憶された時系列の仮の
値の最小固有値の中で所定の値を取る固有値に対応する
相対目標位置データを出力する目標物の位置評価器と、 上記目標物の位置評価器で得られた上記相対目標位置デ
ータを上記ｋ個の可動目標に送信して可動目標の位置を
指示する目標位置指示器と、 上記観測行列生成器出力の観測行列出力と観測雑音の共
分散とからバイアス誤差推定値の共分散行列を算出する
推定値評価器と、上記推定値評価器出力と上記観測行列
出力とから仮のバイアス誤差値を算出する推定値算出器
とを備えて、累積演算結果からセンサのバイアス誤差を
推定するセンサのバイアス誤差推定装置。 - 【請求項２】 誤差分散行列の固有値算出器に替えて、
観測行列生成器出力から観測行列の各要素に連続な関数
である観測行列とその転置行列との積からなる行列の最
小固有値を算出する階数評価値算出器を備えたことを特
徴とする請求項１記載のセンサのバイアス誤差推定装
置。 - 【請求項３】 観測行列生成器出力から観測行列の各要
素に連続な関数である観測行列とその転置行列との積か
らなる行列の最小固有値を算出する階数評価値算出器を
備え、誤差分散行列の固有値算出器と併用したことを特
徴とする請求項１記載のセンサのバイアス誤差推定装
置。 - 【請求項４】 推定値評価器と推定値算出器とに替え
て、観測行列生成器出力の観測行列から観測行列の階数
減少によるバイアス誤差の推定値の発散を抑えるペナル
ティ関数を用いてバイアス誤差を推定するペナルティ関
数による推定値算出器を備えて、演算結果からセンサの
バイアス誤差を推定する請求項１ないし請求項３いずれ
か記載のセンサのバイアス誤差推定装置。 - 【請求項５】 推定値評価器と推定値算出器とに替え
て、観測行列生成器出力の観測行列と観測雑音の共分散
とからバイアス誤差の推定値の発散を抑えるペナルティ
関数を用いてバイアス誤差の推定値の共分散行列を算出
するペナルティ関数型推定値評価器と、上記観測行列と
上記ペナルティ関数型推定値評価器出力とからバイアス
誤差推定値の発散を抑えるペナルティ関数を用いて再帰
的にセンサのバイアス誤差を算出するペナルティ関数型
再帰的推定値算出器を備えて、累積演算結果からセンサ
のバイアス誤差を推定する請求項１ないし請求項３いず
れか記載のセンサのバイアス誤差推定装置。 - 【請求項６】 目標物の相対位置記憶器に替えて、ｋ個
の可動目標に搭載された自己位置測定器からの自己位置
データを、誤差分散行列との仮の値の最小固有値と対応
させて時系列で記憶する可動目標の位置記憶器を備え
て、観測行列生成器出力を用いての演算結果からセンサ
のバイアス誤差を推定する請求項１ないし請求項３いず
れか記載のセンサのバイアス誤差推定装置。 - 【請求項７】 目標物の位置記憶器に替えて、ｋ個の可
動目標に搭載された自己位置測定器からの自己位置デー
タを、誤差分散行列との仮の値の最小固有値と対応させ
て時系列で記憶する可動目標の位置記憶器を備えて、観
測行列生成器出力を用いての演算結果からセンサのバイ
アス誤差を推定する請求項４記載のセンサのバイアス誤
差推定装置。 - 【請求項８】 目標物の位置記憶器に替えて、ｋ個の可
動目標に搭載された自己位置測定器からの自己位置デー
タを、誤差分散行列との仮の値の最小固有値と対応させ
て時系列で記憶する可動目標の位置記憶器を備えて、観
測行列生成器出力を用いての演算結果からセンサのバイ
アス誤差を推定する請求項５記載のセンサのバイアス誤
差推定装置。 - 【請求項９】 ｋ個の可動目標に搭載された自己位置測
定器は、ＧＰＳ(Globl Positioning System)装置である
ことを特徴とする請求項６記載のセンサのバイアス誤差
推定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP10143396A JP3218181B2 (ja) | 1996-04-23 | 1996-04-23 | センサのバイアス誤差推定装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP10143396A JP3218181B2 (ja) | 1996-04-23 | 1996-04-23 | センサのバイアス誤差推定装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH09288160A JPH09288160A (ja) | 1997-11-04 |
| JP3218181B2 true JP3218181B2 (ja) | 2001-10-15 |
Family
ID=14300570
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP10143396A Expired - Fee Related JP3218181B2 (ja) | 1996-04-23 | 1996-04-23 | センサのバイアス誤差推定装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3218181B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4984659B2 (ja) * | 2006-06-05 | 2012-07-25 | 株式会社豊田中央研究所 | 自車両位置推定装置 |
-
1996
- 1996-04-23 JP JP10143396A patent/JP3218181B2/ja not_active Expired - Fee Related
Non-Patent Citations (4)
| Title |
|---|
| Ａ．Ｋｏｎｒａｄ，"Ｔｈｅ ｚｅｒｏ ｄｉｖｅｒｇｅｎｃｅ ｃｏｎｓｔｒａｉｎｔ ｉｎ ３Ｄ ｖｅｃｔｏｒ ｆｉｎｉｔｅ ｅｌｅｍｅｎｔ ｍｅｔｈｏｄｓ．"，Ｖｏｌ．９，Ｓｕｐｐｌｅｍｅｎｔ Ａ，ＣＯＭＰＥＬ，１９９０（米），ｐｐ．７−１６ |
| Ｆｒｅｄｒｉｃ Ｍ．Ｈａｍ他，"Ｏｂｓｅｒｖａｂｉｌｉｔｙ，Ｅｉｇｅｎｖａｌｕｅｓ，ａｎｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒｉｎｇ"，Ｖｏｌ．１９，Ｎｏ．２，ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｅｒｏｓｐａｃｅ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｓｙｓｔｅｍｓ，ＩＥＥＥ，１９８３（米），ｐｐ．２６９−２７３ |
| Ｇ．−Ｗ．Ｙｅ他，"Ｆｉｎｉｔｅ ｅｌｅｍｅｎｔ ａｎａｌｙｓｉｓ ｏｆ ｅｌｅｃｔｒｏｍａｇｎｅｔｉｃ ｆｉｅｌｄ ｆｏｒ ｍｕｌｔｉｄｉｍｅｎｓｉｏｎａｌ ｒｆ ｃａｖｉｔｉｅｓ．"，Ｖｏｌ．６０，Ｎｏ．７，Ｒｅｖｉｅｗ ｏｆ Ｓｃｉｅｎｔｉｆｉｃ Ｉｎｓｔｒｕｍｅｎｔｓ，Ａｍｅｒｉｃａｎ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ｐｈｙｓｉｃｓ，１９８９（米），ｐｐ．１７４０−１７４３ |
| 大矢雅則 他、数理情報科学事典、（株）朝倉書店、１９９５年１１月１０日、ｐ．７９１−７９３ |
Also Published As
| Publication number | Publication date |
|---|---|
| JPH09288160A (ja) | 1997-11-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Xu et al. | A novel DVL calibration method based on robust invariant extended Kalman filter | |
| CN110823217B (zh) | 一种基于自适应联邦强跟踪滤波的组合导航容错方法 | |
| Yang et al. | Comparison of unscented and extended Kalman filters with application in vehicle navigation | |
| US7180443B1 (en) | Reduced state estimator for systems with physically bounded parameters | |
| Youn et al. | Robust interacting multiple model with modeling uncertainties for maneuvering target tracking | |
| JP3179355B2 (ja) | 多目標追尾方法及び多目標追尾装置 | |
| JP2003232845A (ja) | 移動体の方位および姿勢検出装置 | |
| CN110794409A (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
| US7277047B1 (en) | Reduced state estimation with biased measurements | |
| KR101833007B1 (ko) | 도플러 비컨을 이용한 수중 이동체의 위치 및 속도 추정 방법 및 시스템 | |
| Wiktor et al. | Collaborative multi-robot localization in natural terrain | |
| Jingsen et al. | Integrating extreme learning machine with Kalman filter to bridge GPS outages | |
| CN115204212A (zh) | 一种基于stm-pmbm滤波算法的多目标跟踪方法 | |
| Wang et al. | Variational Bayesian cubature RTS smoothing for transfer alignment of DPOS | |
| Wang et al. | A robust backtracking CKF based on Krein space theory for in-motion alignment process | |
| Raol et al. | Sensor data fusion algorithms using square-root information filtering | |
| CN114139109A (zh) | 一种目标跟踪方法、系统、设备、介质及数据处理终端 | |
| Jørgensen et al. | Enhanced hydroacoustic range robustness of three-stage position filter based on long baseline measurements with unknown wave speed | |
| Yuan et al. | Accurate covariance estimation for pose data from iterative closest point algorithm | |
| CN101960322A (zh) | 在三维空间中基于使用声音传感器的粒子滤波器的物体跟踪方法 | |
| KR20120054347A (ko) | Ｓａｒ 영상 형성을 위한 도플러 중심 주파수 정보 추정방법 및 그를 실행시키기 위한 프로그램을 기록한 컴퓨터 판독가능한 매체 | |
| JP3218181B2 (ja) | センサのバイアス誤差推定装置 | |
| Yong et al. | An asynchronous sensor bias estimation algorithm utilizing targets’ positions only | |
| Brekke et al. | Suboptimal Kalman filters for target tracking with navigation uncertainty in one dimension | |
| CN109035301B (zh) | 一种基于斥力模型修正随机矩阵算法的群组目标跟踪方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |