JPH08271247A - センサのアライメント誤差推定装置 - Google Patents
センサのアライメント誤差推定装置Info
- Publication number
- JPH08271247A JPH08271247A JP7256495A JP7256495A JPH08271247A JP H08271247 A JPH08271247 A JP H08271247A JP 7256495 A JP7256495 A JP 7256495A JP 7256495 A JP7256495 A JP 7256495A JP H08271247 A JPH08271247 A JP H08271247A
- Authority
- JP
- Japan
- Prior art keywords
- sensor
- alignment error
- estimated value
- observation
- target
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Length Measuring Devices By Optical Means (AREA)
- Measurement Of Optical Distance (AREA)
Abstract
(57)【要約】
【目的】 センサを２個用いて複数の目標位置を観測す
るとき、片方のセンサが他方のセンサに対してアライメ
ント誤差を持つ。本発明は片方のセンサが他方のセンサ
に対して持つアライメント誤差を推定することを目的と
する。 【構成】 目標の観測位置から観測行列を生成する観測
行列生成器５を設け、さらに、アライメント誤差推定値
の共分散行列を求める推定値評価器６と推定値を算出す
る推定値算出器７を設けてセンサのアライメント誤差を
推定する。 【効果】 センサのアライメント誤差を状態変数とした
状態方程式の解を求めるようにしてアライメント誤差を
推定しているので、観測値に含まれる雑音の影響を受け
ずにセンサのアライメント誤差が得られる。
るとき、片方のセンサが他方のセンサに対してアライメ
ント誤差を持つ。本発明は片方のセンサが他方のセンサ
に対して持つアライメント誤差を推定することを目的と
する。 【構成】 目標の観測位置から観測行列を生成する観測
行列生成器５を設け、さらに、アライメント誤差推定値
の共分散行列を求める推定値評価器６と推定値を算出す
る推定値算出器７を設けてセンサのアライメント誤差を
推定する。 【効果】 センサのアライメント誤差を状態変数とした
状態方程式の解を求めるようにしてアライメント誤差を
推定しているので、観測値に含まれる雑音の影響を受け
ずにセンサのアライメント誤差が得られる。
Description
【０００１】
【産業上の利用分野】この発明は、２つのセンサにより
複数の目標位置を観測し、各々のセンサによる観測結果
に基づいて目標の位置を確認する目標位置観測装置にお
いて、片方のセンサに対する他方のセンサのアライメン
ト誤差を推定する装置に関するものである。
複数の目標位置を観測し、各々のセンサによる観測結果
に基づいて目標の位置を確認する目標位置観測装置にお
いて、片方のセンサに対する他方のセンサのアライメン
ト誤差を推定する装置に関するものである。
【０００２】
【従来の技術】まず、この発明が適用される目標位置観
測装置について図６を用いて説明する。図６は２つのセ
ンサからなる位置観測装置の一構成例であり、図中１は
第１のセンサ、２は第１のセンサ１に対応する第１の観
測器、３は第２のセンサ、４は第２のセンサ２に対応す
る第２の観測器である。１３は観測する目標、１４は第
１のセンサの座標面、１５は第２のセンサ３の第１のセ
ンサ１に対するアライメント誤差である。２つのセンサ
を持つ目標観測装置はこのように構成され、通常片方の
センサが他方のセンサに対してアライメント誤差を持
ち、各々のセンサの座標面が異なることになる。
測装置について図６を用いて説明する。図６は２つのセ
ンサからなる位置観測装置の一構成例であり、図中１は
第１のセンサ、２は第１のセンサ１に対応する第１の観
測器、３は第２のセンサ、４は第２のセンサ２に対応す
る第２の観測器である。１３は観測する目標、１４は第
１のセンサの座標面、１５は第２のセンサ３の第１のセ
ンサ１に対するアライメント誤差である。２つのセンサ
を持つ目標観測装置はこのように構成され、通常片方の
センサが他方のセンサに対してアライメント誤差を持
ち、各々のセンサの座標面が異なることになる。
【０００３】図７は従来のセンサのアライメント誤差の
算出装置の一実施例を示す図である。図中１〜４は図６
と同一である。１６は第２の観測器４からの第２のセン
サ３による目標観測位置から第１の観測器２からの第１
のセンサ１による目標観測位置との差を演算する減算
器、１７は減算器１６の演算結果で第２のセサ３の第１
のセンサ１に対するアライメント誤差である。
算出装置の一実施例を示す図である。図中１〜４は図６
と同一である。１６は第２の観測器４からの第２のセン
サ３による目標観測位置から第１の観測器２からの第１
のセンサ１による目標観測位置との差を演算する減算
器、１７は減算器１６の演算結果で第２のセサ３の第１
のセンサ１に対するアライメント誤差である。
【０００４】次に動作について説明する。従来の装置は
図７において、目標の位置を第１のセンサ１と第１の観
測器２及び第２のセンサ３と第２の観測器４により観測
して第１の減算器１６に送る。減算器１６では第２の観
測器４からの目標観測位置から第１の観測器２からの目
標観測位置を差し引いて差を算出する。減算器１６の演
算結果１７は第２のセンサ３の第１のセンサ１に対する
アライメント誤差であり、これを出力する。
図７において、目標の位置を第１のセンサ１と第１の観
測器２及び第２のセンサ３と第２の観測器４により観測
して第１の減算器１６に送る。減算器１６では第２の観
測器４からの目標観測位置から第１の観測器２からの目
標観測位置を差し引いて差を算出する。減算器１６の演
算結果１７は第２のセンサ３の第１のセンサ１に対する
アライメント誤差であり、これを出力する。
【０００５】
【発明が解決しようとする課題】従来のセンサのアライ
メント誤差算出装置は以上のように構成されているの
で、観測目標の位置による観測誤差及び観測値に含まれ
る観測雑音による影響で算出されるアライメント誤差の
信頼性が低いという問題点があった。
メント誤差算出装置は以上のように構成されているの
で、観測目標の位置による観測誤差及び観測値に含まれ
る観測雑音による影響で算出されるアライメント誤差の
信頼性が低いという問題点があった。
【０００６】この発明は上記のような問題点を解消する
ためになされたもので、第２のセンサの第１のセンサに
対するアライメント誤差を推定することを目的とする。
ためになされたもので、第２のセンサの第１のセンサに
対するアライメント誤差を推定することを目的とする。
【０００７】
【課題を解決するための手段】この発明の実施例１によ
る位置観測装置におけるセンサのアライメント誤差推定
装置は、第１の観測器と第２の観測器とからの目標観測
位置から観測行列を作り、さらに観測行列と観測雑音の
共分散行列とからアライメント誤差推定値の共分散行列
を算出し、目標の観測行列と推定値の共分散行列とから
第２のセンサのアライメント誤差の推定値を算出するも
のである。
る位置観測装置におけるセンサのアライメント誤差推定
装置は、第１の観測器と第２の観測器とからの目標観測
位置から観測行列を作り、さらに観測行列と観測雑音の
共分散行列とからアライメント誤差推定値の共分散行列
を算出し、目標の観測行列と推定値の共分散行列とから
第２のセンサのアライメント誤差の推定値を算出するも
のである。
【０００８】この発明の実施例２による位置観測装置に
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらにアライ
メント誤差の推定値を算出するときの初期値を設定し、
目標の観測行列と推定値の共分散行列とアライメント誤
差初期値とから第２のセンサのアライメント誤差の推定
値を算出するものである。
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらにアライ
メント誤差の推定値を算出するときの初期値を設定し、
目標の観測行列と推定値の共分散行列とアライメント誤
差初期値とから第２のセンサのアライメント誤差の推定
値を算出するものである。
【０００９】この発明の実施例３による位置観測装置に
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらに算出し
たアライメント誤差を記憶し、目標の観測行列と推定値
の共分散行列と前回算出したアライメント誤差の推定値
を用いて第２のセンサのアライメント誤差の推定値を算
出するものである。
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらに算出し
たアライメント誤差を記憶し、目標の観測行列と推定値
の共分散行列と前回算出したアライメント誤差の推定値
を用いて第２のセンサのアライメント誤差の推定値を算
出するものである。
【００１０】この発明の実施例４による位置観測装置に
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらにアライ
メント誤差の推定値を算出するときの初期値を設定し、
また算出したアライメント誤差の初期値を記憶し、この
記憶したアライメント誤差と目標の観測行列と推定値の
共分散行列とアライメント誤差初期値とから第２のセン
サのアライメント誤差の推定値を算出するものである。
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらにアライ
メント誤差の推定値を算出するときの初期値を設定し、
また算出したアライメント誤差の初期値を記憶し、この
記憶したアライメント誤差と目標の観測行列と推定値の
共分散行列とアライメント誤差初期値とから第２のセン
サのアライメント誤差の推定値を算出するものである。
【００１１】この発明の実施例５による位置観測装置に
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらにアライ
メント誤差の推定値を算出するときの初期値を設定し、
目標の観測行列と推定値の共分散行列とアライメント誤
差初期値とから第２のセンサのアライメント誤差の推定
値を算出し、さらに算出した推定値の収束具合を判定す
るものである。
おけるセンサのアライメント誤差推定装置は、第１の観
測器と第２の観測器とからの目標観測位置から観測行列
を作り、観測行列と観測雑音の共分散行列とからアライ
メント誤差推定値の共分散行列を算出し、さらにアライ
メント誤差の推定値を算出するときの初期値を設定し、
目標の観測行列と推定値の共分散行列とアライメント誤
差初期値とから第２のセンサのアライメント誤差の推定
値を算出し、さらに算出した推定値の収束具合を判定す
るものである。
【００１２】
【作用】この発明の実施例１によるセンサのアライメン
ト誤差推定装置は、目標観測装置が目標を観測するとき
の観測行列を算出し、アライメント誤差推定値の共分散
行列を算出し、第２のセンサのアライメント誤差を状態
変数とした状態方程式の解としてアライメント誤差を推
定することができる。
ト誤差推定装置は、目標観測装置が目標を観測するとき
の観測行列を算出し、アライメント誤差推定値の共分散
行列を算出し、第２のセンサのアライメント誤差を状態
変数とした状態方程式の解としてアライメント誤差を推
定することができる。
【００１３】この発明の実施例２によるセンサのアライ
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式の解を、上記状態方程式の初
期値を設定し、設定した初期値を用いてアライメント誤
差の推定値を上記状態方程式の解として算出することが
できる。
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式の解を、上記状態方程式の初
期値を設定し、設定した初期値を用いてアライメント誤
差の推定値を上記状態方程式の解として算出することが
できる。
【００１４】この発明の実施例３によるセンサのアライ
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式を仮定し、算出した解を記憶
しておき、その解を用いてアライメント誤差を推定値を
上記状態方程式の解として繰り返し演算を行なうことで
算出することができる。
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式を仮定し、算出した解を記憶
しておき、その解を用いてアライメント誤差を推定値を
上記状態方程式の解として繰り返し演算を行なうことで
算出することができる。
【００１５】この発明の実施例４によるセンサのアライ
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式を仮定し、算出した解を記憶
しておき、その解と上記状態方程式の初期値を設定しそ
の初期値を用いて、アライメント誤差の推定値を上記状
態方程式の解として繰り返し演算を行なうことで算出す
ることができる。
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式を仮定し、算出した解を記憶
しておき、その解と上記状態方程式の初期値を設定しそ
の初期値を用いて、アライメント誤差の推定値を上記状
態方程式の解として繰り返し演算を行なうことで算出す
ることができる。
【００１６】この発明の実施例５によるセンサのアライ
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式を仮定し、算出した解を記憶
しておき、その解と上記状態方程式の初期値を設定しそ
の初期値を用いて、アライメント誤差の推定値を上記状
態方程式の解として繰り返し演算を行なうことで算出す
ることができ、さらに算出された状態方程式の解の収束
具合を判定できる。
メント誤差推定装置は、目標観測装置が目標を観測する
ときの観測行列を算出し、アライメント誤差推定値の共
分散行列を算出し、第２のセンサのアライメント誤差を
状態変数とした状態方程式を仮定し、算出した解を記憶
しておき、その解と上記状態方程式の初期値を設定しそ
の初期値を用いて、アライメント誤差の推定値を上記状
態方程式の解として繰り返し演算を行なうことで算出す
ることができ、さらに算出された状態方程式の解の収束
具合を判定できる。
【００１７】
実施例１ 図１はこの発明の一実施例を示すセンサのバイアス誤差
算出装置の構成図である。図中１〜４は従来の装置と同
一である。５は第１の観測器２と第２の観測器４からの
目標観測位置から目標の観測行列を算出する観測行列生
成器、６は観測行列からアライメント誤差の推定値の共
分散行列を算出する推定値評価器、７は観測行列生成器
５からの観測行列と推定値評価器６からのアライメント
誤差の推定値の共分散行列とから第２のセンサ３の第１
のセンサ１に対するアライメント誤差推定値を算出する
推定値算出器、８は第２のセンサ３の第１のセンサ１に
対するアライメント誤差推定値である。
算出装置の構成図である。図中１〜４は従来の装置と同
一である。５は第１の観測器２と第２の観測器４からの
目標観測位置から目標の観測行列を算出する観測行列生
成器、６は観測行列からアライメント誤差の推定値の共
分散行列を算出する推定値評価器、７は観測行列生成器
５からの観測行列と推定値評価器６からのアライメント
誤差の推定値の共分散行列とから第２のセンサ３の第１
のセンサ１に対するアライメント誤差推定値を算出する
推定値算出器、８は第２のセンサ３の第１のセンサ１に
対するアライメント誤差推定値である。
【００１８】次に動作について説明する。第１のセンサ
１及び第２のセンサ３から２以上の任意の数をｋとして
ｋ番目の目標を見たときの目標の正しい位置を各々Ｐ１
ｋｔ，Ｐ２ｋｔとおき、各々のセンサから得られる観測
位置を各々Ｐ１ｋｔＯ，Ｐ２ｋｔＯとおく。また、第２
のセンサ３の第１のセンサ１に対するアライメント誤差
をｄ（ｄＲ，ｄＥ，ｄＢ）とし第１のセンサ１からみた
第２のセンサ３の位置ベクトルをＰｐとする。
１及び第２のセンサ３から２以上の任意の数をｋとして
ｋ番目の目標を見たときの目標の正しい位置を各々Ｐ１
ｋｔ，Ｐ２ｋｔとおき、各々のセンサから得られる観測
位置を各々Ｐ１ｋｔＯ，Ｐ２ｋｔＯとおく。また、第２
のセンサ３の第１のセンサ１に対するアライメント誤差
をｄ（ｄＲ，ｄＥ，ｄＢ）とし第１のセンサ１からみた
第２のセンサ３の位置ベクトルをＰｐとする。
【００１９】Ｐ１ｋｔ，Ｐ２ｋｔ，Ｐ１ｋｔＯ，Ｐ２ｋ
ｔＯは直交座標系で表せば、各々数１〜数４となる。
ｔＯは直交座標系で表せば、各々数１〜数４となる。
【００２０】
【数１】
【００２１】
【数２】
【００２２】
【数３】
【００２３】
【数４】
【００２４】また、明らかに数５が成り立つ。
【００２５】
【数５】
【００２６】ここで、関数Ｆとして数６を定義する。
【００２７】
【数６】
【００２８】数６で表される関数Ｆを、第１のセンサ１
に対する第２のセンサのアライメント誤差の初期値をｄ
（０）＝（０，０，０）として、点（Ｐ１ｋｔＯ，Ｐ２
ｋｔＯ，ｄ（０））のまわりでテイラー展開し、２次以
上の項を無視すると、数７が得られる。
に対する第２のセンサのアライメント誤差の初期値をｄ
（０）＝（０，０，０）として、点（Ｐ１ｋｔＯ，Ｐ２
ｋｔＯ，ｄ（０））のまわりでテイラー展開し、２次以
上の項を無視すると、数７が得られる。
【００２９】
【数７】
【００３０】数７中のＧｉｋｔ（ｉ＝１または２）は数
８と表される。
８と表される。
【００３１】
【数８】
【００３２】関数Ｆは第１のセンサ１に対する第２のセ
ンサ２の位置ベクトルＰｐに誤差がなければ恒等的に零
になる。従って、数７の右辺を零とおいて数９を得る。
ンサ２の位置ベクトルＰｐに誤差がなければ恒等的に零
になる。従って、数７の右辺を零とおいて数９を得る。
【００３３】
【数９】
【００３４】数９の左辺は、アライメント誤差の初期値
と観測位置により定まる観測値と考えられる。数９の右
辺第１項は求めるべきアライメント誤差を状態変数とし
て状態変数と観測行列Ｇ２ｋｔとの積と考えられ、第２
項及び第３項は観測位置誤差と観測行列の積、即ち、雑
音と考えられる。従って、数９は観測系に雑音が含まれ
る線形状態方程式と考えることができる。
と観測位置により定まる観測値と考えられる。数９の右
辺第１項は求めるべきアライメント誤差を状態変数とし
て状態変数と観測行列Ｇ２ｋｔとの積と考えられ、第２
項及び第３項は観測位置誤差と観測行列の積、即ち、雑
音と考えられる。従って、数９は観測系に雑音が含まれ
る線形状態方程式と考えることができる。
【００３５】いま、観測値をＺ、観測行列をＨ、求める
状態変数をｘとおき、目標観測時の雑音をｖとおけば、
数１０と表される。
状態変数をｘとおき、目標観測時の雑音をｖとおけば、
数１０と表される。
【００３６】
【数１０】
【００３７】最小自乗法を用いれば観測値と状態変数ｘ
との差の自乗を評価関数Ｊにすれば良い。目標観測時の
雑音ｖの共分散をＲとおけばＪは数１１となる。
との差の自乗を評価関数Ｊにすれば良い。目標観測時の
雑音ｖの共分散をＲとおけばＪは数１１となる。
【００３８】
【数１１】
【００３９】評価関数Ｊを最小にするｘは、数１１の右
辺を展開して、ｘについて微分した結果が零になるよう
なｘである。従って求める状態変数ｘは数１２と表され
る。
辺を展開して、ｘについて微分した結果が零になるよう
なｘである。従って求める状態変数ｘは数１２と表され
る。
【００４０】
【数１２】
【００４１】ここで、Ｐ（＋）は推定値の共分散行列で
ある。
ある。
【００４２】次に、この発明を図１に従って説明する。
第１のセンサ１と第２のセンサ３はｋ個の目標の位置を
観測するためのセンサで、第１のセンサ１に対応する第
１の観測器２と第２のセンサ３に対応する第２の観測器
４により目標の観測位置を出力する。観測行列生成器５
では、上記第１の観測器２及び第２の観測器４からの目
標の観測位置をうけて数７、数８で表される観測行列Ｇ
１ｋｔ、Ｇ２ｋｔを演算して求める。推定値評価器６で
は観測行列生成器５からの観測行列Ｇ１ｋｔ、Ｇ２ｋｔ
から数１２中のＰ（＋）、即ち、推定値の共分散行列を
演算する。推定値算出器７では推定値評価器６からの推
定値の共分散行列と観測行列生成器５からの観測行列と
から数１２で表される求める推定値ｘの右辺第２項を算
出する。数１２導出の過程でｄ（０）＝（０、０、０）
としているので、推定値算出器７の出力が求めるセンサ
２のセンサ１に対するアライメント誤差の推定値８とな
る。
第１のセンサ１と第２のセンサ３はｋ個の目標の位置を
観測するためのセンサで、第１のセンサ１に対応する第
１の観測器２と第２のセンサ３に対応する第２の観測器
４により目標の観測位置を出力する。観測行列生成器５
では、上記第１の観測器２及び第２の観測器４からの目
標の観測位置をうけて数７、数８で表される観測行列Ｇ
１ｋｔ、Ｇ２ｋｔを演算して求める。推定値評価器６で
は観測行列生成器５からの観測行列Ｇ１ｋｔ、Ｇ２ｋｔ
から数１２中のＰ（＋）、即ち、推定値の共分散行列を
演算する。推定値算出器７では推定値評価器６からの推
定値の共分散行列と観測行列生成器５からの観測行列と
から数１２で表される求める推定値ｘの右辺第２項を算
出する。数１２導出の過程でｄ（０）＝（０、０、０）
としているので、推定値算出器７の出力が求めるセンサ
２のセンサ１に対するアライメント誤差の推定値８とな
る。
【００４３】このように目標の観測位置を用いて、第２
のセンサ３の第１のセンサ１に対するアライメント誤差
を状態変数とした状態方程式を仮定してアライメント誤
差を推定しているので、目標の位置による観測誤差及び
観測位置に含まれる観測雑音の影響を受けにくい。
のセンサ３の第１のセンサ１に対するアライメント誤差
を状態変数とした状態方程式を仮定してアライメント誤
差を推定しているので、目標の位置による観測誤差及び
観測位置に含まれる観測雑音の影響を受けにくい。
【００４４】実施例２ 図２はこの発明の一実施例を示すセンサのアライメント
誤差算出装置の構成図である。図中１〜８は実施例１と
同一である。９は第２のセンサの第１のセンサに対する
アライメント誤差の推定計算を行なうときの初期値を設
定するアライメント誤差初期値設定器である。
誤差算出装置の構成図である。図中１〜８は実施例１と
同一である。９は第２のセンサの第１のセンサに対する
アライメント誤差の推定計算を行なうときの初期値を設
定するアライメント誤差初期値設定器である。
【００４５】次に動作について説明する。第１のセンサ
１及び第２のセンサ３により目標の位置を観測し推定値
評価器６でアライメント誤差推定値の共分散行列を算出
するまでの動作は実施例１と同一である。アライメント
誤差初期値設定器９では推定値算出器７でアライメント
誤差の推定値計算を行なうときの初期値、即ち数１２の
ｄ（０）を設定するものである。推定値算出器７では観
測行列生成器５からの観測行列と推定値評価器６からの
推定値の共分散行列とアライメント誤差初期値設定器９
からの初期値とから数１２で表される推定値ｘを算出す
る。
１及び第２のセンサ３により目標の位置を観測し推定値
評価器６でアライメント誤差推定値の共分散行列を算出
するまでの動作は実施例１と同一である。アライメント
誤差初期値設定器９では推定値算出器７でアライメント
誤差の推定値計算を行なうときの初期値、即ち数１２の
ｄ（０）を設定するものである。推定値算出器７では観
測行列生成器５からの観測行列と推定値評価器６からの
推定値の共分散行列とアライメント誤差初期値設定器９
からの初期値とから数１２で表される推定値ｘを算出す
る。
【００４６】このように第２のセンサ３の第１のセンサ
１に対するアライメント誤差の推定計算の初期値を設定
できるようにしているので、何らかの方法によりアライ
メント誤差の初期値を得て、この実施例に示すように設
定すると、より精度良くアライメント誤差を推定でき
る。
１に対するアライメント誤差の推定計算の初期値を設定
できるようにしているので、何らかの方法によりアライ
メント誤差の初期値を得て、この実施例に示すように設
定すると、より精度良くアライメント誤差を推定でき
る。
【００４７】実施例３ 図３はこの発明の一実施例を示すセンサのアライメント
誤差算出装置の構成図である。図中１〜８は実施例１と
同一である。１０は推定値算出器７からのアライメント
誤差推定値８と推定値の共分散行列Ｐ（＋）を記憶して
おく推定値記憶器である。
誤差算出装置の構成図である。図中１〜８は実施例１と
同一である。１０は推定値算出器７からのアライメント
誤差推定値８と推定値の共分散行列Ｐ（＋）を記憶して
おく推定値記憶器である。
【００４８】次に動作について説明する。第１のセンサ
１と第２のセンサ３により目標の位置を観測し観測行列
生成器５で観測行列を算出するまでの処理は実施例１と
同一である。次に、前回算出のアライメント誤差推定値
をｘ（−）、前回推定値の共分散行列をＰ（−）とお
く。最小自乗法を用いれば、求めるべきアライメント誤
差の推定値をｘとして、ｘと前回算出のアライメント誤
差推定値ｘ（−）との差の自乗、及び、観測値ｚとｘと
の差の自乗との和を評価関数Ｊ１にすれば良いので、Ｊ
１は数１３と表される。
１と第２のセンサ３により目標の位置を観測し観測行列
生成器５で観測行列を算出するまでの処理は実施例１と
同一である。次に、前回算出のアライメント誤差推定値
をｘ（−）、前回推定値の共分散行列をＰ（−）とお
く。最小自乗法を用いれば、求めるべきアライメント誤
差の推定値をｘとして、ｘと前回算出のアライメント誤
差推定値ｘ（−）との差の自乗、及び、観測値ｚとｘと
の差の自乗との和を評価関数Ｊ１にすれば良いので、Ｊ
１は数１３と表される。
【００４９】
【数１３】
【００５０】Ｊ１を最小にするｘはＪ１の右辺を展開し
てｘについて微分した結果が零になるようなｘである。
従って求める推定値ｘは数１４と表される。
てｘについて微分した結果が零になるようなｘである。
従って求める推定値ｘは数１４と表される。
【００５１】
【数１４】
【００５２】この発明を図３に従って説明する。第１の
センサ１と第１の観測器２及び第２のセンサ３と第２の
観測器４により目標の位置を観測して観測行列生成器５
で目標を観測するときの観測行列を算出するまでの動作
は実施例１と同一である。推定値評価器６では前回算出
の推定値の共分散行列をＰ（−）と観測行列生成器５か
らの観測行列と目標観測時の観測雑音ｖの共分散行列Ｒ
とから数１４で表される推定値の共分散行列Ｐ（＋）を
算出する。推定値算出器７では観測行列生成器５からの
観測行列と推定値評価器６からの推定値の共分散行列Ｐ
（＋）と推定値記憶器１０からの前回算出のバイアス誤
差推定値ｘ（−）とから数１４のｘで表されるセンサ２
のセンサ１に対するアライメント誤差推定値８を算出す
る。算出されたアライメント誤差の推定値８及び推定値
の共分散行列Ｐ（＋）は推定値記憶器１０に送出されて
次回アライメント誤差の推定値を算出するときに前回推
定値ｘ（−）及び前回算出の共分散行列Ｐ（−）として
用いられる。
センサ１と第１の観測器２及び第２のセンサ３と第２の
観測器４により目標の位置を観測して観測行列生成器５
で目標を観測するときの観測行列を算出するまでの動作
は実施例１と同一である。推定値評価器６では前回算出
の推定値の共分散行列をＰ（−）と観測行列生成器５か
らの観測行列と目標観測時の観測雑音ｖの共分散行列Ｒ
とから数１４で表される推定値の共分散行列Ｐ（＋）を
算出する。推定値算出器７では観測行列生成器５からの
観測行列と推定値評価器６からの推定値の共分散行列Ｐ
（＋）と推定値記憶器１０からの前回算出のバイアス誤
差推定値ｘ（−）とから数１４のｘで表されるセンサ２
のセンサ１に対するアライメント誤差推定値８を算出す
る。算出されたアライメント誤差の推定値８及び推定値
の共分散行列Ｐ（＋）は推定値記憶器１０に送出されて
次回アライメント誤差の推定値を算出するときに前回推
定値ｘ（−）及び前回算出の共分散行列Ｐ（−）として
用いられる。
【００５３】このように目標の位置を観測したときに、
目標の観測位置と前回算出した推定値とからバイアス誤
差を収束計算するので精度がよい。
目標の観測位置と前回算出した推定値とからバイアス誤
差を収束計算するので精度がよい。
【００５４】実施例４ 図４はこの発明の一実施例を示すセンサのアライメント
誤差推定装置の一実施例である。図中、１〜８及び１０
は実施例３と同一である。９はセンサのアライメント誤
差推定計算の初期値を設定するアライメント誤差初期値
設定器である。
誤差推定装置の一実施例である。図中、１〜８及び１０
は実施例３と同一である。９はセンサのアライメント誤
差推定計算の初期値を設定するアライメント誤差初期値
設定器である。
【００５５】次に動作について説明する。第１のセンサ
１と第１の観測器２及び第２のセンサ３と第２の観測器
４により目標の位置を観測してセンサ２のセンサ１に対
するアライメント誤差推定値８を算出するまでの処理は
実施例３と同一である。アライメント誤差初期値設定器
９では推定値算出器７でアライメント誤差の推定値計算
を行なうときの初期値、即ち数１４のｘ（−）の初期値
を設定するもので、アライメント誤差推定値の収束計算
の初期値となる。アライメント誤差初期値設定器９から
のアライメント誤差初期値は推定値記憶器１０に送られ
て記憶され、初めて（第１回目）のアライメント誤差収
束計算に用いられる。推定値算出器７では観測行列生成
器５からの観測行列と制定地表下記６からの推定値の共
分散行列と推定値記憶器１０からの前回計算時のアライ
メント誤差推定値、または、初めて（第１回目）の推定
計算の時はアライメント誤差の初期値とから数１４で表
される推定値ｘを算出してアライメント誤差の推定値８
として出力する。
１と第１の観測器２及び第２のセンサ３と第２の観測器
４により目標の位置を観測してセンサ２のセンサ１に対
するアライメント誤差推定値８を算出するまでの処理は
実施例３と同一である。アライメント誤差初期値設定器
９では推定値算出器７でアライメント誤差の推定値計算
を行なうときの初期値、即ち数１４のｘ（−）の初期値
を設定するもので、アライメント誤差推定値の収束計算
の初期値となる。アライメント誤差初期値設定器９から
のアライメント誤差初期値は推定値記憶器１０に送られ
て記憶され、初めて（第１回目）のアライメント誤差収
束計算に用いられる。推定値算出器７では観測行列生成
器５からの観測行列と制定地表下記６からの推定値の共
分散行列と推定値記憶器１０からの前回計算時のアライ
メント誤差推定値、または、初めて（第１回目）の推定
計算の時はアライメント誤差の初期値とから数１４で表
される推定値ｘを算出してアライメント誤差の推定値８
として出力する。
【００５６】このように推定値の収束計算を行なうとき
の初期値を設定できるので、何らかの方法によりアライ
メント誤差の初期値を設定すると、バイアス誤差の推定
値を早く収束させることができ、さらに精度良くアライ
メント誤差を推定できる。
の初期値を設定できるので、何らかの方法によりアライ
メント誤差の初期値を設定すると、バイアス誤差の推定
値を早く収束させることができ、さらに精度良くアライ
メント誤差を推定できる。
【００５７】実施例５ 図５はこの発明の一実施例を示すセンサのアライメント
誤差推定装置の一実施例である。図中、１〜１０は実施
例４と同一である。１１はセンサのアライメント誤差の
推定値の収束の具合を判定する推定値収束判定器、１２
は推定値収束判定器１１の出力でセンサのアライメント
誤差の推定値算出結果である。
誤差推定装置の一実施例である。図中、１〜１０は実施
例４と同一である。１１はセンサのアライメント誤差の
推定値の収束の具合を判定する推定値収束判定器、１２
は推定値収束判定器１１の出力でセンサのアライメント
誤差の推定値算出結果である。
【００５８】次に動作について説明する。第１のセンサ
１と第１の観測器２及び第２のセンサ３と第２の観測器
４により目標の位置を観測してセンサ２のセンサ１に対
するアライメント誤差推定値８を算出し、推定値記憶器
１０で算出したアライメント誤差を記憶するまでの処理
は実施例４と同一である。推定値収束判定器１１では前
回算出したアライメント誤差の推定値と今回算出したア
ライメント誤差の推定値とを比較してアライメント誤差
の推定値８の収束具合を監視する。推定値収束判定器１
１はアライメント誤差の推定値がある値以下に収束して
いるときに、推定値計算が収束したものとしてアライメ
ント誤差の推定値８を推定値算出結果１２として出力す
る。
１と第１の観測器２及び第２のセンサ３と第２の観測器
４により目標の位置を観測してセンサ２のセンサ１に対
するアライメント誤差推定値８を算出し、推定値記憶器
１０で算出したアライメント誤差を記憶するまでの処理
は実施例４と同一である。推定値収束判定器１１では前
回算出したアライメント誤差の推定値と今回算出したア
ライメント誤差の推定値とを比較してアライメント誤差
の推定値８の収束具合を監視する。推定値収束判定器１
１はアライメント誤差の推定値がある値以下に収束して
いるときに、推定値計算が収束したものとしてアライメ
ント誤差の推定値８を推定値算出結果１２として出力す
る。
【００５９】このようにアライメント誤差の推定値を収
束計算し、さらに収束判定をしているので得られる推定
値算出結果の精度がよい。
束計算し、さらに収束判定をしているので得られる推定
値算出結果の精度がよい。
【００６０】
【発明の効果】この発明の実施例１および２によればセ
ンサのアライメント誤差を状態変数とした状態方程式を
導出し、その解を算出することでアライメント誤差を算
出できるという効果がある。
ンサのアライメント誤差を状態変数とした状態方程式を
導出し、その解を算出することでアライメント誤差を算
出できるという効果がある。
【００６１】この発明の実施例３および４によればセン
サのアライメント誤差を状態変数とした状態方程式を導
出し、解を算出する毎にその解を記憶し、その解を用い
て収束計算にてセンサのアライメント誤差を算出してい
るので精度良くセンサのバイアス誤差が推定できるとい
う効果がある。
サのアライメント誤差を状態変数とした状態方程式を導
出し、解を算出する毎にその解を記憶し、その解を用い
て収束計算にてセンサのアライメント誤差を算出してい
るので精度良くセンサのバイアス誤差が推定できるとい
う効果がある。
【００６２】この発明の実施例５によればセンサのアラ
イメント誤差を状態変数とした状態方程式を導出し、解
を算出する毎にその解を記憶し、その解を用いて収束計
算にてセンサのアライメント誤差を算出し、その解の収
束具合を判定できるという効果がある。
イメント誤差を状態変数とした状態方程式を導出し、解
を算出する毎にその解を記憶し、その解を用いて収束計
算にてセンサのアライメント誤差を算出し、その解の収
束具合を判定できるという効果がある。
【図１】この発明の実施例１を示すセンサのアライメン
ト誤差推定装置の構成図である。
ト誤差推定装置の構成図である。
【図２】この発明の実施例２を示すセンサのアライメン
ト誤差推定装置の構成図である。
ト誤差推定装置の構成図である。
【図３】この発明の実施例３を示すセンサのアライメン
ト誤差推定装置の構成図である。
ト誤差推定装置の構成図である。
【図４】この発明の実施例４を示すセンサのアライメン
ト誤差推定装置の構成図である。
ト誤差推定装置の構成図である。
【図５】この発明の実施例５を示すセンサのアライメン
ト誤差推定装置の構成図である。
ト誤差推定装置の構成図である。
【図６】２つのセンサからなる位置観測装置の一構成
例。
例。
【図７】従来のセンサのアライメント誤差算出装置の構
成図。
成図。
１ 第１のセンサ ２ 第１の観測器 ３ 第２のセンサ ４ 第２の観測器 ５ 観測行列生成器 ６ 推定値評価器 ７ 推定値算出器 ８ アライメント誤差推定値 ９ アライメント誤差初期値設定器 １０ 推定値記憶器 １１ 推定値収束判定器 １２ 推定値算出結果 １３ 観測目標 １４ 第１のセンサの座標面 １５ 第２のセンサの第１のセンサに対するアライメン
ト誤差 １６ 減算器 １７ アライメント誤差算出結果
ト誤差 １６ 減算器 １７ アライメント誤差算出結果
Claims (5)
- 【請求項１】 複数の目標位置を観測する第１のセンサ
と、この第１のセンサに対応する第１の観測器と、同じ
複数の目標位置を観測する第２のセンサと、この第２の
センサに対応する第２の観測器とから構成され、２つの
センサにより観測した結果に基づいて目標の位置を確認
する目標位置観測装置において、上記第１のセンサと第
１の観測器によって観測された目標観測位置と上記第２
のセンサと第２の観測器により観測された目標観測位置
とから観測行列を演算する観測行列生成器と、目標観測
位置に含まれる観測雑音の共分散と観測行列とからアラ
イメント誤差推定値の共分散行列を算出する推定値評価
器と、上記の観測行列と推定値評価器からの共分散行列
とから第２のセンサのアライメント誤差の値を算出する
推定値算出器とから構成され、第２のセンサの第１のセ
ンサに対するアライメント誤差を推定することを特徴と
するセンサのアライメント誤差推定装置。 - 【請求項２】 第２のセンサの第１のセンサに対するア
ライメント誤差推定値を算出するときの初期値を設定す
るアライメント誤差初期値設定器を備え、推定計算の初
期値を推定値算出器に送出することを特徴とする請求項
１記載のセンサのアライメント誤差推定装置。 - 【請求項３】 第２のセンサの第１のセンサに対するア
ライメント誤差推定値を算出する推定値算出器の出力を
記憶しておく推定値記憶器を備え、推定値記憶器の内容
を推定値評価器及び推定値算出器に送出することを特徴
とする請求項１記載のセンサのアライメント誤差推定装
置。 - 【請求項４】 第２のセンサの第１のセンサに対するア
ライメント誤差推定値を算出するときの初期値を設定す
るアライメント誤差初期値設定器を備え、推定計算の初
期値を推定値記憶器に送出することを特徴とする請求項
３記載のセンサのアライメント誤差推定装置。 - 【請求項５】 第２のセンサの第１のセンサに対するア
ライメント誤差を算出する推定値算出器の出力により推
定値の収束を判定する推定値収束判定器を備え、アライ
メント誤差推定値が収束すると推定値を算出結果として
出力することを特徴とした請求項４記載のセンサのアラ
イメント誤差推定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP7256495A JPH08271247A (ja) | 1995-03-30 | 1995-03-30 | センサのアライメント誤差推定装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP7256495A JPH08271247A (ja) | 1995-03-30 | 1995-03-30 | センサのアライメント誤差推定装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH08271247A true JPH08271247A (ja) | 1996-10-18 |
Family
ID=13492993
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP7256495A Pending JPH08271247A (ja) | 1995-03-30 | 1995-03-30 | センサのアライメント誤差推定装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH08271247A (ja) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004347471A (ja) * | 2003-05-22 | 2004-12-09 | Fuji Heavy Ind Ltd | 先行車両検出装置 |
| JP2011047703A (ja) * | 2009-08-25 | 2011-03-10 | Mitsutoyo Corp | 誤差伝播による出力データの精度評価方法 |
| JP2014153211A (ja) * | 2013-02-08 | 2014-08-25 | Furukawa Electric Co Ltd:The | 周辺監視システム及び周辺監視システムの軸ずれ検知方法 |
| JPWO2021024712A1 (ja) * | 2019-08-02 | 2021-02-11 | ||
| WO2023243374A1 (ja) * | 2022-06-17 | 2023-12-21 | ソニーグループ株式会社 | 情報処理装置、情報処理方法、及び、測定システム |
-
1995
- 1995-03-30 JP JP7256495A patent/JPH08271247A/ja active Pending
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2004347471A (ja) * | 2003-05-22 | 2004-12-09 | Fuji Heavy Ind Ltd | 先行車両検出装置 |
| JP2011047703A (ja) * | 2009-08-25 | 2011-03-10 | Mitsutoyo Corp | 誤差伝播による出力データの精度評価方法 |
| JP2014153211A (ja) * | 2013-02-08 | 2014-08-25 | Furukawa Electric Co Ltd:The | 周辺監視システム及び周辺監視システムの軸ずれ検知方法 |
| JPWO2021024712A1 (ja) * | 2019-08-02 | 2021-02-11 | ||
| WO2021024712A1 (ja) * | 2019-08-02 | 2021-02-11 | 日立オートモティブシステムズ株式会社 | エイミング装置、運転制御システム、及びセンサデータの補正量の計算方法 |
| WO2023243374A1 (ja) * | 2022-06-17 | 2023-12-21 | ソニーグループ株式会社 | 情報処理装置、情報処理方法、及び、測定システム |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP2169422B1 (en) | System and method for acoustic tracking an underwater vehicle trajectory | |
| JP7027365B2 (ja) | 信号処理装置、信号処理方法およびプログラム | |
| US7277047B1 (en) | Reduced state estimation with biased measurements | |
| CN111623703A (zh) | 一种基于新型卡尔曼滤波的北斗变形监测实时处理方法 | |
| JP6361225B2 (ja) | 車両の運動性能解析方法 | |
| JPH08271247A (ja) | センサのアライメント誤差推定装置 | |
| US8213265B2 (en) | Method of object tracking in 3D space based on particle filter using acoustic sensors | |
| CN115278496A (zh) | 用于麦克风阵列测量的稀疏声源辨识方法、系统 | |
| Giordano et al. | Consistency aspects of Wiener-Hammerstein model identification in presence of process noise | |
| US20100246893A1 (en) | Method and Apparatus for Nonlinear Dynamic Estimation of Feature Depth Using Calibrated Moving Cameras | |
| CN110736468A (zh) | 自适应运动学模型辅助的航天器姿态估计方法 | |
| JP3439068B2 (ja) | センサ姿勢及び位置のバイアス誤差推定装置 | |
| JPH07151532A (ja) | センサのバイアス誤差推定装置 | |
| JPH06273173A (ja) | センサのバイアス誤差推定装置 | |
| JPH09281206A (ja) | 目標位置観測装置 | |
| US20210385425A1 (en) | Method and device for calibrating binocular camera | |
| CN112747773A (zh) | 基于Allan方差和随机多项式提高陀螺仪精度的方法 | |
| JP2001091622A (ja) | 目標位置観測装置 | |
| CN114236524A (zh) | 多普勒雷达序贯平滑变结构滤波方法及装置 | |
| CN110208791B (zh) | 一种纯角度跟踪伪线性滤波方法 | |
| JPH0599649A (ja) | センサのバイアス誤差推定装置 | |
| CN113625262A (zh) | 目标轨迹确定方法及相关设备 | |
| JPH0553648A (ja) | 位置観測装置のバイアス誤差推定装置 | |
| JP3302870B2 (ja) | センサのバイアス誤差推定装置 | |
| JPH0694823A (ja) | センサのバイアス誤差推定装置 |