JPH10319108A - センサ姿勢及び位置のバイアス誤差推定装置 - Google Patents
センサ姿勢及び位置のバイアス誤差推定装置Info
- Publication number
- JPH10319108A JPH10319108A JP9129549A JP12954997A JPH10319108A JP H10319108 A JPH10319108 A JP H10319108A JP 9129549 A JP9129549 A JP 9129549A JP 12954997 A JP12954997 A JP 12954997A JP H10319108 A JPH10319108 A JP H10319108A
- Authority
- JP
- Japan
- Prior art keywords
- sensor
- bias error
- attitude
- observation
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
(57)【要約】
【課題】 第１，第２のセンサの位置の計測値にバイア
ス誤差が含まれる場合にも、第１，第２のセンサの位置
のバイアス誤差及び姿勢のバイアス誤差を同時に精度よ
く求めるセンサ姿勢及び位置のバイアス誤差推定装置を
得ること。 【解決手段】 観測行列生成器９と、センサ姿勢及び位
置のバイアス誤差推定算出器１１を設け、２つのセンサ
１，２の姿勢及び位置のバイアス誤差を状態変数とした
線形状態方程式の観測行列を算出し、センサの姿勢及び
位置のバイアス誤差を推定しているので各センサの姿勢
及び位置のバイアス誤差を同時に精度よく求めることが
できる。
ス誤差が含まれる場合にも、第１，第２のセンサの位置
のバイアス誤差及び姿勢のバイアス誤差を同時に精度よ
く求めるセンサ姿勢及び位置のバイアス誤差推定装置を
得ること。 【解決手段】 観測行列生成器９と、センサ姿勢及び位
置のバイアス誤差推定算出器１１を設け、２つのセンサ
１，２の姿勢及び位置のバイアス誤差を状態変数とした
線形状態方程式の観測行列を算出し、センサの姿勢及び
位置のバイアス誤差を推定しているので各センサの姿勢
及び位置のバイアス誤差を同時に精度よく求めることが
できる。
Description
【０００１】
【発明の属する技術分野】この発明は、複数のセンサを
持つ目標位置観測装置におけるセンサ姿勢及び位置のバ
イアス誤差推定装置に関するもので、各センサの姿勢及
び位置のバイアス誤差の精度のよい推定に関する。
持つ目標位置観測装置におけるセンサ姿勢及び位置のバ
イアス誤差推定装置に関するもので、各センサの姿勢及
び位置のバイアス誤差の精度のよい推定に関する。
【０００２】
【従来の技術】図５は、例えば特開平６−９４８２３号
公報に示された従来のセンサのバイアス誤差推定装置を
示す図である。図において、１は第１のセンサ、２は第
２のセンサ、３は第１の観測器、４は第２の観測器、２
０は第１のセンサ１と第２のセンサ２の位置を入力する
センサ位置設定器、６は第１の観測器３からの目標位置
とセンサ位置設定器２０からの第１のセンサ位置を加算
して目標の観測位置を算出する第１の加算器、７は第２
の観測器４からの目標位置とセンサ位置設定器１９から
の第２のセンサ位置を加算して目標の観測位置を算出す
る第２の加算器、２１は第１のセンサ１及び第２のセン
サ２の姿勢のバイアス誤差の初期値を定めるセンサ姿勢
初期バイアス誤差設定器、２２は第１の加算器６と第２
の加算器７からの目標観測位置とセンサ姿勢初期バイア
ス誤差設定器２１からのセンサ姿勢のバイアス誤差初期
値とからｋ個の目標の観測行列を算出する観測行列生成
器、２３は観測行列からバイアス誤差の推定値の共分散
行列を算出するセンサ姿勢のバイアス誤差推定値評価
器、２４は観測行列とセンサ姿勢のバイアス誤差推定値
評価器２３からのセンサ姿勢のバイアス誤差の推定値の
共分散行列とからサンプリング時刻ｔにおけるセンサの
バイアス誤差推定値の仮の値を算出するセンサ姿勢のバ
イアス誤差推定値算出器、２５はセンサ姿勢の初期バイ
アス誤差設定器２１からのセンサ姿勢のバイアス誤差の
初期値よりセンサ姿勢のバイアス誤差推定値算出器２４
からのサンプリング時刻ｔにおけるセンサ姿勢のバイア
ス誤差推定値の仮の値を差し引き、時刻ｔにおける第１
のセンサ１と第２のセンサ２の姿勢のバイアス誤差推定
値を出力する減算器、２７は減算器２５からの第１のセ
ンサと第２のセンサの時刻ｔにおけるセンサ姿勢のバイ
アス誤差推定値を記憶しておくセンサ姿勢のバイアス誤
差推定値記憶器である。
公報に示された従来のセンサのバイアス誤差推定装置を
示す図である。図において、１は第１のセンサ、２は第
２のセンサ、３は第１の観測器、４は第２の観測器、２
０は第１のセンサ１と第２のセンサ２の位置を入力する
センサ位置設定器、６は第１の観測器３からの目標位置
とセンサ位置設定器２０からの第１のセンサ位置を加算
して目標の観測位置を算出する第１の加算器、７は第２
の観測器４からの目標位置とセンサ位置設定器１９から
の第２のセンサ位置を加算して目標の観測位置を算出す
る第２の加算器、２１は第１のセンサ１及び第２のセン
サ２の姿勢のバイアス誤差の初期値を定めるセンサ姿勢
初期バイアス誤差設定器、２２は第１の加算器６と第２
の加算器７からの目標観測位置とセンサ姿勢初期バイア
ス誤差設定器２１からのセンサ姿勢のバイアス誤差初期
値とからｋ個の目標の観測行列を算出する観測行列生成
器、２３は観測行列からバイアス誤差の推定値の共分散
行列を算出するセンサ姿勢のバイアス誤差推定値評価
器、２４は観測行列とセンサ姿勢のバイアス誤差推定値
評価器２３からのセンサ姿勢のバイアス誤差の推定値の
共分散行列とからサンプリング時刻ｔにおけるセンサの
バイアス誤差推定値の仮の値を算出するセンサ姿勢のバ
イアス誤差推定値算出器、２５はセンサ姿勢の初期バイ
アス誤差設定器２１からのセンサ姿勢のバイアス誤差の
初期値よりセンサ姿勢のバイアス誤差推定値算出器２４
からのサンプリング時刻ｔにおけるセンサ姿勢のバイア
ス誤差推定値の仮の値を差し引き、時刻ｔにおける第１
のセンサ１と第２のセンサ２の姿勢のバイアス誤差推定
値を出力する減算器、２７は減算器２５からの第１のセ
ンサと第２のセンサの時刻ｔにおけるセンサ姿勢のバイ
アス誤差推定値を記憶しておくセンサ姿勢のバイアス誤
差推定値記憶器である。
【０００３】次に原理について説明する。第１のセンサ
１、第２のセンサ２からｋ個の目標を見たときのｋ番目
の目標の正しい位置ベクトルは以下の式（１）のように
極座標で表わされる。
１、第２のセンサ２からｋ個の目標を見たときのｋ番目
の目標の正しい位置ベクトルは以下の式（１）のように
極座標で表わされる。
【０００４】
【数１】
【０００５】また、実際に各々のセンサから得られる観
測位置ベクトルは以下の式（２）のように極座標で表わ
される。
測位置ベクトルは以下の式（２）のように極座標で表わ
される。
【０００６】
【数２】
【０００７】また、第１のセンサ１と第２のセンサ２の
姿勢のバイアス誤差を式（３）として、
姿勢のバイアス誤差を式（３）として、
【０００８】
【数３】
【０００９】第１のセンサ１の設置位置を原点、第２の
センサ２の位置ベクトルを以下の式（４）のように直交
座標で表わす。
センサ２の位置ベクトルを以下の式（４）のように直交
座標で表わす。
【００１０】
【数４】
【００１１】極座標で表わされた目標の正しい位置ベク
トルと、実際に各々のセンサから得られる観測位置ベク
トルを直交座標で表わすと各々以下の式（５）、（６）
となる。
トルと、実際に各々のセンサから得られる観測位置ベク
トルを直交座標で表わすと各々以下の式（５）、（６）
となる。
【００１２】
【数５】
【００１３】また、明らかに式（７）が成り立つ。
【００１４】
【数６】
【００１５】ここで関数Ｇとして式（８）を定義して、
【００１６】
【数７】
【００１７】第１のセンサ１と第２のセンサ２のバイア
ス誤差の１サンプリング前の推定値として式（９）を関
数Ｇの平衡点の近傍に選んで、
ス誤差の１サンプリング前の推定値として式（９）を関
数Ｇの平衡点の近傍に選んで、
【００１８】
【数８】
【００１９】関数Ｇを（Ｐ1ko （ｔ）アンダーバー，Ｐ
2ko （ｔ）アンダーバー，θ1′，θ2′）のまわりでテ
イラー展開し、２次以上の項を無視すると、式（１０）
が得られる。
2ko （ｔ）アンダーバー，θ1′，θ2′）のまわりでテ
イラー展開し、２次以上の項を無視すると、式（１０）
が得られる。
【００２０】
【数９】
【００２１】ただし、式（１１）とする。
【００２２】
【数１０】
【００２３】関数Ｇは第１のセンサ１と第２のセンサ２
の観測ベクトルＰ1ko（ｔ）アンダーバー，Ｐ2ko（ｔ）
アンダーバーに誤差がなければ恒等的に零になる。従っ
て、式（１０）の右辺を零とおいて、式（１２）が得ら
れる。
の観測ベクトルＰ1ko（ｔ）アンダーバー，Ｐ2ko（ｔ）
アンダーバーに誤差がなければ恒等的に零になる。従っ
て、式（１０）の右辺を零とおいて、式（１２）が得ら
れる。
【００２４】
【数１１】
【００２５】式（１２）の左辺は、センサ姿勢のバイア
ス誤差の初期値と観測位置により定まる観測値と考えら
れる。式（１２）の右辺第１項、第２項は求めるべきセ
ンサ姿勢のバイアス誤差を状態変数として、観測行列と
の積と考えられる。第３項、第４項は観測位置誤差と観
測行列の積、即ち、雑音と考えられる。従って、式（１
２）は観測系に雑音が含まれる線形状態方程式と考える
ことができる。
ス誤差の初期値と観測位置により定まる観測値と考えら
れる。式（１２）の右辺第１項、第２項は求めるべきセ
ンサ姿勢のバイアス誤差を状態変数として、観測行列と
の積と考えられる。第３項、第４項は観測位置誤差と観
測行列の積、即ち、雑音と考えられる。従って、式（１
２）は観測系に雑音が含まれる線形状態方程式と考える
ことができる。
【００２６】即ち、観測値をｚt アンダーバー，観測行
列をＨt ，求める状態変数をｘアンダーバー，観測雑音
ベクトルをνt アンダーバーとおけば、次式（１３）が
得られる。
列をＨt ，求める状態変数をｘアンダーバー，観測雑音
ベクトルをνt アンダーバーとおけば、次式（１３）が
得られる。
【００２７】
【数１２】
【００２８】ここで、各項は式（１４）ないし式（１
８）で表される。
８）で表される。
【００２９】
【数１３】
【００３０】式（１３）の状態ベクトルｘアンダーバー
はサンプリング時刻によらない定数ベクトルなので、カ
ルマンフィルタの理論において推移行列を単位行列、駆
動雑音を零ベクトルとして、センサ姿勢のバイアス誤差
を推定することができる。サンプリング時刻ｔにおける
状態ベクトルｘアンダーバーの推定結果を式（１９）と
書けば、
はサンプリング時刻によらない定数ベクトルなので、カ
ルマンフィルタの理論において推移行列を単位行列、駆
動雑音を零ベクトルとして、センサ姿勢のバイアス誤差
を推定することができる。サンプリング時刻ｔにおける
状態ベクトルｘアンダーバーの推定結果を式（１９）と
書けば、
【００３１】
【数１４】
【００３２】式（２０）ないし式（２２）となる。
【００３３】
【数１５】
【００３４】ここで、式（２３）ないし式（２８）が成
立する。
立する。
【００３５】
【数１６】
【００３６】なお、（σ2 itR（ｔ），σ2 itE（ｔ），σ
2 itAz（ｔ））は観測誤差の分散である。
2 itAz（ｔ））は観測誤差の分散である。
【００３７】次に動作について説明する。図７おいて、
第１のセンサ１、第２のセンサ２は目標の位置を観測す
るためのセンサで、同時に目標位置をサンプルする。第
１の観測器３、第２の観測器４により式（２）で表され
る目標の位置を出力する。また、センサ位置設定器２０
はある基準位置（以下，基準位置と呼ぶ）に対する第１
のセンサ１と第２のセンサ２の位置を設定する。ここ
で、第１のセンサ１と第２のセンサ２の位置関係は式
（７）で表される。第１の加算器６、第２の加算器７で
は、第１の観測器３、第２の観測器４からの目標の位置
に第１のセンサ１と第２のセンサ２の位置を加算し、基
準位置に対する目標の観測位置を算出する。また、セン
サ姿勢の初期バイアス誤差設定器２１は動作開始時（ｔ
＝１）では第１のセンサ１、第２のセンサ２の姿勢のバ
イアス誤差初期値を設定し、それ以後（ｔ＞１）はセン
サ姿勢のバイアス誤差推定値記憶器２７からの前回算出
のセンサ姿勢のバイアス誤差推定値を出力する（即ち式
（２０）の右辺第１項を出力する）。
第１のセンサ１、第２のセンサ２は目標の位置を観測す
るためのセンサで、同時に目標位置をサンプルする。第
１の観測器３、第２の観測器４により式（２）で表され
る目標の位置を出力する。また、センサ位置設定器２０
はある基準位置（以下，基準位置と呼ぶ）に対する第１
のセンサ１と第２のセンサ２の位置を設定する。ここ
で、第１のセンサ１と第２のセンサ２の位置関係は式
（７）で表される。第１の加算器６、第２の加算器７で
は、第１の観測器３、第２の観測器４からの目標の位置
に第１のセンサ１と第２のセンサ２の位置を加算し、基
準位置に対する目標の観測位置を算出する。また、セン
サ姿勢の初期バイアス誤差設定器２１は動作開始時（ｔ
＝１）では第１のセンサ１、第２のセンサ２の姿勢のバ
イアス誤差初期値を設定し、それ以後（ｔ＞１）はセン
サ姿勢のバイアス誤差推定値記憶器２７からの前回算出
のセンサ姿勢のバイアス誤差推定値を出力する（即ち式
（２０）の右辺第１項を出力する）。
【００３８】観測行列生成器２２では、上記第１の加算
器６、第２の加算器７からの目標の観測位置と、センサ
姿勢の初期バイアス誤差設定器２１からのセンサ姿勢の
バイアス誤差初期値または前回算出のセンサ姿勢のバイ
アス誤差推定値を受けて式（１５）で表わされる観測行
列Ｈtを演算して求める。これは、式（１６）のＦ
k（ｔ）に相当する。センサ姿勢のバイアス誤差推定値
評価器２３では式（２２）における共分散行列Ｐt を演
算する。さらに、センサ姿勢のバイアス誤差推定値算出
器２４ではセンサ姿勢のバイアス誤差推定値評価器２３
からの推定値の共分散行列を用いて、サンプリング時刻
ｔにおけるセンサ姿勢のバイアス誤差推定値の仮の値、
即ち式（２０）の右辺第２項を算出する。さらに減算器
２５でセンサ姿勢のバイアス誤差推定値算出器２４から
のセンサ姿勢のバイアス誤差推定値をセンサ姿勢の初期
バイアス誤差設定器２１からのセンサ姿勢のバイアス誤
差初期値またはサンプリング時刻（ｔ−１）のセンサ姿
勢のバイアス誤差推定値、即ち式（２０）の右辺第１項
から差し引いて、式（２０）のセンサ姿勢のバイアス誤
差推定値を演算し出力するとともに、センサ姿勢のバイ
アス誤差推定値記憶器２７に送出する。センサ姿勢のバ
イアス誤差推定値記憶器２７は、算出したサンプリング
時刻ｔにおけるセンサ姿勢のバイアス誤差の推定値を記
憶するとともに、前回算出のセンサ姿勢のバイアス誤差
推定値としてセンサ姿勢の初期バイアス誤差設定器２１
に送出する。以後、この一連の処理をセンサ姿勢のバイ
アス誤差の推定値が収束するまで繰り返す。なお、この
ときセンサ姿勢の初期バイアス誤差設定器２１からはそ
の都度、前回（サンプリング時刻（ｔ−１)）算出のセ
ンサのバイアス誤差推定値が出力される。
器６、第２の加算器７からの目標の観測位置と、センサ
姿勢の初期バイアス誤差設定器２１からのセンサ姿勢の
バイアス誤差初期値または前回算出のセンサ姿勢のバイ
アス誤差推定値を受けて式（１５）で表わされる観測行
列Ｈtを演算して求める。これは、式（１６）のＦ
k（ｔ）に相当する。センサ姿勢のバイアス誤差推定値
評価器２３では式（２２）における共分散行列Ｐt を演
算する。さらに、センサ姿勢のバイアス誤差推定値算出
器２４ではセンサ姿勢のバイアス誤差推定値評価器２３
からの推定値の共分散行列を用いて、サンプリング時刻
ｔにおけるセンサ姿勢のバイアス誤差推定値の仮の値、
即ち式（２０）の右辺第２項を算出する。さらに減算器
２５でセンサ姿勢のバイアス誤差推定値算出器２４から
のセンサ姿勢のバイアス誤差推定値をセンサ姿勢の初期
バイアス誤差設定器２１からのセンサ姿勢のバイアス誤
差初期値またはサンプリング時刻（ｔ−１）のセンサ姿
勢のバイアス誤差推定値、即ち式（２０）の右辺第１項
から差し引いて、式（２０）のセンサ姿勢のバイアス誤
差推定値を演算し出力するとともに、センサ姿勢のバイ
アス誤差推定値記憶器２７に送出する。センサ姿勢のバ
イアス誤差推定値記憶器２７は、算出したサンプリング
時刻ｔにおけるセンサ姿勢のバイアス誤差の推定値を記
憶するとともに、前回算出のセンサ姿勢のバイアス誤差
推定値としてセンサ姿勢の初期バイアス誤差設定器２１
に送出する。以後、この一連の処理をセンサ姿勢のバイ
アス誤差の推定値が収束するまで繰り返す。なお、この
ときセンサ姿勢の初期バイアス誤差設定器２１からはそ
の都度、前回（サンプリング時刻（ｔ−１)）算出のセ
ンサのバイアス誤差推定値が出力される。
【００３９】
【発明が解決しようとする課題】従来のセンサ姿勢のバ
イアス誤差推定装置では、第１及び第２のセンサの正し
い位置が得られているものとして、センサ姿勢のバイア
ス誤差を推定している。ところが、実際は第１及び第２
のセンサの位置の計測値には誤差が含まれるため、この
誤差の大きさによってはセンサ姿勢のバイアス誤差の推
定値が著しく劣化するという問題があった。
イアス誤差推定装置では、第１及び第２のセンサの正し
い位置が得られているものとして、センサ姿勢のバイア
ス誤差を推定している。ところが、実際は第１及び第２
のセンサの位置の計測値には誤差が含まれるため、この
誤差の大きさによってはセンサ姿勢のバイアス誤差の推
定値が著しく劣化するという問題があった。
【００４０】この発明は上記のような課題を解決するた
めになされたもので、第１及び第２のセンサの位置の計
測値にバイアス誤差が含まれる場合にも、第１及び第２
のセンサの位置のバイアス誤差及び姿勢のバイアス誤差
を同時に精度よく求めるセンサ姿勢及び位置のバイアス
誤差推定装置を得ることを目的とする。
めになされたもので、第１及び第２のセンサの位置の計
測値にバイアス誤差が含まれる場合にも、第１及び第２
のセンサの位置のバイアス誤差及び姿勢のバイアス誤差
を同時に精度よく求めるセンサ姿勢及び位置のバイアス
誤差推定装置を得ることを目的とする。
【００４１】
【課題を解決するための手段】上記の目的を達成するた
めに、請求項１に係るセンサ姿勢及び位置バイアス誤差
推定装置は、互いに離れた位置にある２つのセンサから
観測器を介してｋ個の目標の位置を観測し各センサを原
点とする直交座標に変換した出力と、上記２つのセンサ
の位置を基準直交座標にて計測したセンサ位置計測器の
出力とを加算して、上記基準直交座標にて上記目標の観
測位置を求めて一方の入力とし、上記センサ自体に起因
しその観測諸元である目標の距離，仰角，方位角各々に
含まれるバイアス誤差をセンサ姿勢のバイアス誤差と定
義し、上記センサ位置の計測値に含まれるバイアス誤差
をセンサ位置のバイアス誤差と定義し、それらセンサ姿
勢及び位置のバイアス誤差の初期値を設定するセンサ姿
勢及び位置の初期バイアス誤差設定器の出力を他方の入
力として、それらセンサ姿勢及び位置のバイアス誤差を
状態変数とした線形状態方程式の観測行列を算出する観
測行列生成手段と、上記観測行列からセンサ姿勢及び位
置のバイアス誤差推定値の仮の値を算出するセンサ姿勢
及び位置のバイアス誤差推定値算出手段と、上記センサ
姿勢及び位置のバイアス誤差の初期値から上記センサ姿
勢及び位置のバイアス誤差推定値の仮の値を差し引いて
センサ姿勢及び位置のバイアス誤差推定値を出力する手
段と、を備えたことを特徴とする。
めに、請求項１に係るセンサ姿勢及び位置バイアス誤差
推定装置は、互いに離れた位置にある２つのセンサから
観測器を介してｋ個の目標の位置を観測し各センサを原
点とする直交座標に変換した出力と、上記２つのセンサ
の位置を基準直交座標にて計測したセンサ位置計測器の
出力とを加算して、上記基準直交座標にて上記目標の観
測位置を求めて一方の入力とし、上記センサ自体に起因
しその観測諸元である目標の距離，仰角，方位角各々に
含まれるバイアス誤差をセンサ姿勢のバイアス誤差と定
義し、上記センサ位置の計測値に含まれるバイアス誤差
をセンサ位置のバイアス誤差と定義し、それらセンサ姿
勢及び位置のバイアス誤差の初期値を設定するセンサ姿
勢及び位置の初期バイアス誤差設定器の出力を他方の入
力として、それらセンサ姿勢及び位置のバイアス誤差を
状態変数とした線形状態方程式の観測行列を算出する観
測行列生成手段と、上記観測行列からセンサ姿勢及び位
置のバイアス誤差推定値の仮の値を算出するセンサ姿勢
及び位置のバイアス誤差推定値算出手段と、上記センサ
姿勢及び位置のバイアス誤差の初期値から上記センサ姿
勢及び位置のバイアス誤差推定値の仮の値を差し引いて
センサ姿勢及び位置のバイアス誤差推定値を出力する手
段と、を備えたことを特徴とする。
【００４２】また、請求項２に係るセンサ姿勢及び位置
バイアス誤差推定装置は、互いに離れた位置にある２つ
のセンサ位置の地球の半径を設定する曲率設定器と、上
記２つのセンサから観測器を介してｋ個の目標の位置を
観測し各センサを原点とする直交座標のｘ座標及びｙ座
標と、海面からの高さを用いた基平面座標に変換した出
力と、上記２つのセンサの位置を基準の基平面座標にて
計測したセンサ位置計測器の出力とを加算して、上記基
準の基平面座標にて上記目標の観測位置を求めて一方の
入力とし、上記センサ自体に起因しその観測諸元である
目標の距離，仰角，方位角各々に含まれるバイアス誤差
をセンサ姿勢のバイアス誤差と定義し、上記センサ位置
の計測値に含まれるバイアス誤差をセンサ位置のバイア
ス誤差と定義し、それらセンサ姿勢及び位置のバイアス
誤差の初期値を設定するセンサ姿勢及び位置の初期バイ
アス誤差設定器の出力を他方の入力として、それらセン
サ姿勢及び位置のバイアス誤差を状態変数とした線形状
態方程式の観測行列を算出する観測行列生成手段と、上
記観測行列からセンサ姿勢及び位置のバイアス誤差推定
値の仮の値を算出するセンサ姿勢及び位置のバイアス誤
差推定値算出手段と、上記センサ姿勢及び位置のバイア
ス誤差の初期値から上記センサ姿勢及び位置のバイアス
誤差推定値の仮の値を差し引いてセンサ姿勢及び位置の
バイアス誤差推定値を出力する手段と、を備えたことを
特徴とする。
バイアス誤差推定装置は、互いに離れた位置にある２つ
のセンサ位置の地球の半径を設定する曲率設定器と、上
記２つのセンサから観測器を介してｋ個の目標の位置を
観測し各センサを原点とする直交座標のｘ座標及びｙ座
標と、海面からの高さを用いた基平面座標に変換した出
力と、上記２つのセンサの位置を基準の基平面座標にて
計測したセンサ位置計測器の出力とを加算して、上記基
準の基平面座標にて上記目標の観測位置を求めて一方の
入力とし、上記センサ自体に起因しその観測諸元である
目標の距離，仰角，方位角各々に含まれるバイアス誤差
をセンサ姿勢のバイアス誤差と定義し、上記センサ位置
の計測値に含まれるバイアス誤差をセンサ位置のバイア
ス誤差と定義し、それらセンサ姿勢及び位置のバイアス
誤差の初期値を設定するセンサ姿勢及び位置の初期バイ
アス誤差設定器の出力を他方の入力として、それらセン
サ姿勢及び位置のバイアス誤差を状態変数とした線形状
態方程式の観測行列を算出する観測行列生成手段と、上
記観測行列からセンサ姿勢及び位置のバイアス誤差推定
値の仮の値を算出するセンサ姿勢及び位置のバイアス誤
差推定値算出手段と、上記センサ姿勢及び位置のバイア
ス誤差の初期値から上記センサ姿勢及び位置のバイアス
誤差推定値の仮の値を差し引いてセンサ姿勢及び位置の
バイアス誤差推定値を出力する手段と、を備えたことを
特徴とする。
【００４３】また、請求項３に係るセンサ姿勢及び位置
バイアス誤差推定装置は、互いに離れた位置にある２つ
のセンサから観測器を介してｋ個の目標の位置を観測し
各センサを原点とする直交座標に変換した出力と、上記
２つのセンサの位置を基準直交座標にて計測したセンサ
位置計測器の出力とを加算して、上記基準直交座標にて
上記目標の観測位置を求めて一方の入力とし、上記セン
サ自体に起因しその観測諸元である目標の距離，仰角，
方位角各々に含まれるバイアス誤差をセンサ姿勢のバイ
アス誤差と定義し、上記センサ位置の計測値に含まれる
バイアス誤差をセンサ位置のバイアス誤差と定義し、そ
れらセンサ姿勢及び位置のバイアス誤差の初期値を設定
するセンサ姿勢及び位置の初期バイアス誤差設定器の出
力を他方の入力として、それらセンサ姿勢及び位置のバ
イアス誤差を状態変数とした線形状態方程式の観測行列
を算出する観測行列生成手段と、上記観測行列からセン
サ姿勢及び位置のバイアス誤差推定値の共分散行列を算
出するセンサ姿勢及び位置のバイアス誤差推定値評価手
段と、上記観測行列と上記センサ姿勢及び位置のバイア
ス誤差推定値の共分散行列とからセンサ姿勢及び位置の
バイアス誤差推定値の仮の値を算出するセンサ姿勢及び
位置のバイアス誤差推定値算出手段と、上記センサ姿勢
及び位置のバイアス誤差の初期値もしくは前回のバイア
ス誤差推定値からセンサ姿勢及び位置のバイアス誤差推
定値の仮の値を差し引いて累積演算結果からセンサ姿勢
及び位置のバイアス誤差推定値を出力する手段と、を備
えたことを特徴とする。
バイアス誤差推定装置は、互いに離れた位置にある２つ
のセンサから観測器を介してｋ個の目標の位置を観測し
各センサを原点とする直交座標に変換した出力と、上記
２つのセンサの位置を基準直交座標にて計測したセンサ
位置計測器の出力とを加算して、上記基準直交座標にて
上記目標の観測位置を求めて一方の入力とし、上記セン
サ自体に起因しその観測諸元である目標の距離，仰角，
方位角各々に含まれるバイアス誤差をセンサ姿勢のバイ
アス誤差と定義し、上記センサ位置の計測値に含まれる
バイアス誤差をセンサ位置のバイアス誤差と定義し、そ
れらセンサ姿勢及び位置のバイアス誤差の初期値を設定
するセンサ姿勢及び位置の初期バイアス誤差設定器の出
力を他方の入力として、それらセンサ姿勢及び位置のバ
イアス誤差を状態変数とした線形状態方程式の観測行列
を算出する観測行列生成手段と、上記観測行列からセン
サ姿勢及び位置のバイアス誤差推定値の共分散行列を算
出するセンサ姿勢及び位置のバイアス誤差推定値評価手
段と、上記観測行列と上記センサ姿勢及び位置のバイア
ス誤差推定値の共分散行列とからセンサ姿勢及び位置の
バイアス誤差推定値の仮の値を算出するセンサ姿勢及び
位置のバイアス誤差推定値算出手段と、上記センサ姿勢
及び位置のバイアス誤差の初期値もしくは前回のバイア
ス誤差推定値からセンサ姿勢及び位置のバイアス誤差推
定値の仮の値を差し引いて累積演算結果からセンサ姿勢
及び位置のバイアス誤差推定値を出力する手段と、を備
えたことを特徴とする。
【００４４】また、請求項４に係るセンサ姿勢及び位置
バイアス誤差推定装置は、互いに離れた位置にある２つ
のセンサ位置の地球の半径を設定する曲率設定器と、上
記２つのセンサから観測器を介してｋ個の目標の位置を
観測し各センサを原点とする直交座標のｘ座標及びｙ座
標と、海面からの高さを用いた基平面座標に変換した出
力と、上記２つのセンサの位置を基準の基平面座標にて
計測したセンサ位置計測器の出力とを加算して、上記基
準の基平面座標にて上記目標の観測位置を求めて一方の
入力とし、上記センサ自体に起因しその観測諸元である
目標の距離，仰角，方位角各々に含まれるバイアス誤差
をセンサ姿勢のバイアス誤差と定義し、上記センサ位置
の計測値に含まれるバイアス誤差をセンサ位置のバイア
ス誤差と定義し、それらセンサの姿勢及び位置のバイア
ス誤差の初期値を設定するセンサ姿勢及び位置の初期バ
イアス誤差設定器の出力を他方の入力として、それらセ
ンサの姿勢及び位置のバイアス誤差を状態変数とした線
形状態方程式の観測行列を算出する観測行列生成手段
と、上記観測行列からセンサ姿勢及び位置のバイアス誤
差推定値の共分散行列を算出するセンサ姿勢及び位置の
バイアス誤差推定値評価手段と、上記観測行列と上記セ
ンサ姿勢及び位置のバイアス誤差推定値の共分散行列と
からセンサ姿勢及び位置のバイアス誤差推定値の仮の値
を算出するセンサ姿勢及び位置のバイアス誤差推定手段
と、上記センサ姿勢及び位置のバイアス誤差の初期値も
しくは前回のバイアス誤差推定値からセンサ姿勢及び位
置のバイアス誤差推定値の仮の値を差し引いて累積演算
結果からセンサ姿勢及び位置のバイアス誤差推定値を出
力する手段と、を備えたことを特徴とする。
バイアス誤差推定装置は、互いに離れた位置にある２つ
のセンサ位置の地球の半径を設定する曲率設定器と、上
記２つのセンサから観測器を介してｋ個の目標の位置を
観測し各センサを原点とする直交座標のｘ座標及びｙ座
標と、海面からの高さを用いた基平面座標に変換した出
力と、上記２つのセンサの位置を基準の基平面座標にて
計測したセンサ位置計測器の出力とを加算して、上記基
準の基平面座標にて上記目標の観測位置を求めて一方の
入力とし、上記センサ自体に起因しその観測諸元である
目標の距離，仰角，方位角各々に含まれるバイアス誤差
をセンサ姿勢のバイアス誤差と定義し、上記センサ位置
の計測値に含まれるバイアス誤差をセンサ位置のバイア
ス誤差と定義し、それらセンサの姿勢及び位置のバイア
ス誤差の初期値を設定するセンサ姿勢及び位置の初期バ
イアス誤差設定器の出力を他方の入力として、それらセ
ンサの姿勢及び位置のバイアス誤差を状態変数とした線
形状態方程式の観測行列を算出する観測行列生成手段
と、上記観測行列からセンサ姿勢及び位置のバイアス誤
差推定値の共分散行列を算出するセンサ姿勢及び位置の
バイアス誤差推定値評価手段と、上記観測行列と上記セ
ンサ姿勢及び位置のバイアス誤差推定値の共分散行列と
からセンサ姿勢及び位置のバイアス誤差推定値の仮の値
を算出するセンサ姿勢及び位置のバイアス誤差推定手段
と、上記センサ姿勢及び位置のバイアス誤差の初期値も
しくは前回のバイアス誤差推定値からセンサ姿勢及び位
置のバイアス誤差推定値の仮の値を差し引いて累積演算
結果からセンサ姿勢及び位置のバイアス誤差推定値を出
力する手段と、を備えたことを特徴とする。
【００４５】
実施の形態１．図１はこの発明の実施の形態１を示すセ
ンサ姿勢及び位置のバイアス誤差推定装置の構成図であ
る。図において、１〜４及び６〜７は従来の装置と同一
である。５は第１及び第２のセンサの位置を計測するセ
ンサ位置計測器、８は第１及び第２のセンサの姿勢及び
位置のバイアス誤差初期値を定めるセンサ姿勢及び位置
の初期バイアス誤差設定器、９は第１の加算器６からの
目標観測位置と、第２の加算器７からの目標観測位置
と、センサ姿勢及び位置の初期バイアス誤差設定器８か
らのセンサ姿勢及び位置のバイアス誤差初期値とから目
標の観測行列を算出する観測行列生成器、１１は観測行
列生成器９からの目標の観測行列からセンサ姿勢及び位
置のバイアス誤差推定値の仮の値を算出するセンサ姿勢
及び位置のバイアス誤差推定値算出器、１２はセンサ姿
勢及び位置の初期バイアス誤差設定器８からのセンサ姿
勢及び位置のバイアス誤差初期値よりセンサ姿勢及び位
置のバイアス誤差推定値算出器１１からのセンサ姿勢及
び位置のバイアス誤差推定値の仮の値を差し引き、セン
サ姿勢及び位置のバイアス誤差推定値を出力する減算器
である。図２は基準点に対する目標位置ベクトルを説明
する図である。
ンサ姿勢及び位置のバイアス誤差推定装置の構成図であ
る。図において、１〜４及び６〜７は従来の装置と同一
である。５は第１及び第２のセンサの位置を計測するセ
ンサ位置計測器、８は第１及び第２のセンサの姿勢及び
位置のバイアス誤差初期値を定めるセンサ姿勢及び位置
の初期バイアス誤差設定器、９は第１の加算器６からの
目標観測位置と、第２の加算器７からの目標観測位置
と、センサ姿勢及び位置の初期バイアス誤差設定器８か
らのセンサ姿勢及び位置のバイアス誤差初期値とから目
標の観測行列を算出する観測行列生成器、１１は観測行
列生成器９からの目標の観測行列からセンサ姿勢及び位
置のバイアス誤差推定値の仮の値を算出するセンサ姿勢
及び位置のバイアス誤差推定値算出器、１２はセンサ姿
勢及び位置の初期バイアス誤差設定器８からのセンサ姿
勢及び位置のバイアス誤差初期値よりセンサ姿勢及び位
置のバイアス誤差推定値算出器１１からのセンサ姿勢及
び位置のバイアス誤差推定値の仮の値を差し引き、セン
サ姿勢及び位置のバイアス誤差推定値を出力する減算器
である。図２は基準点に対する目標位置ベクトルを説明
する図である。
【００４６】次に原理について説明する。ある基準点に
対するセンサの位置を直交座標（基準直交座標）で計測
するセンサ位置計測器５によって時刻ｔに計測された第
１及び第２のセンサの位置ベクトル（位置計測ベクト
ル）を式（２９）とし、便宜上これを式（３０）のよう
にまとめて表す。ここで、センサ位置計測器５による第
１及び第２のセンサの位置の計測値に式（３１）のバイ
アス誤差（位置のバイアス誤差）とランダム誤差が含ま
れているとし、便宜上、２つのセンサの位置のバイアス
誤差を式（３２）のようにまとめて表す。また、第１及
び第２のセンサの位置計測ベクトルを式（３３）とし、
各センサの真の位置ベクトルを式（３４）で表すと、式
（３５）の関係がある。ここで、便宜上、式（３４）を
式（３６）のようにまとめて表す。また、ＡTはベクト
ルＡの転置ベクトルを示す。
対するセンサの位置を直交座標（基準直交座標）で計測
するセンサ位置計測器５によって時刻ｔに計測された第
１及び第２のセンサの位置ベクトル（位置計測ベクト
ル）を式（２９）とし、便宜上これを式（３０）のよう
にまとめて表す。ここで、センサ位置計測器５による第
１及び第２のセンサの位置の計測値に式（３１）のバイ
アス誤差（位置のバイアス誤差）とランダム誤差が含ま
れているとし、便宜上、２つのセンサの位置のバイアス
誤差を式（３２）のようにまとめて表す。また、第１及
び第２のセンサの位置計測ベクトルを式（３３）とし、
各センサの真の位置ベクトルを式（３４）で表すと、式
（３５）の関係がある。ここで、便宜上、式（３４）を
式（３６）のようにまとめて表す。また、ＡTはベクト
ルＡの転置ベクトルを示す。
【００４７】
【数１７】
【００４８】一方、第１及び第２のセンサからｋ個の目
標を観測して得られる目標の位置ベクトル（以下、観測
ベクトルと呼ぶ）はそれぞれ各センサを原点とする極座
標（距離、仰角、方位角）で表される。第１及び第２の
センサによる時刻ｔにおけるｋ番目の目標の観測ベクト
ルを式（３７）とし、便宜上これを式（３８）のように
まとめて表す。このとき第１のセンサ及び第２のセンサ
の姿勢にずれがあり、各センサの観測諸元である距離、
仰角、方位角のそれぞれに式（３９）のバイアス誤差
（姿勢のバイアス誤差）とランダム誤差が含まれるとす
る。便宜上、２つのセンサの姿勢のバイアス誤差を式
（４０）のようにまとめて表す。また、ランダム誤差を
除いた第１及び第２のセンサによる目標の観測ベクトル
を式（４１）とする。また、各センサから見たｋ番目の
目標の真の位置ベクトルを式（４２）とし、便宜上これ
を式（４３）のようにまとめて表す。ここで、式（４
２）をそれぞれ各センサの位置を原点とする直交座標に
変換すると式（４４）で表される。
標を観測して得られる目標の位置ベクトル（以下、観測
ベクトルと呼ぶ）はそれぞれ各センサを原点とする極座
標（距離、仰角、方位角）で表される。第１及び第２の
センサによる時刻ｔにおけるｋ番目の目標の観測ベクト
ルを式（３７）とし、便宜上これを式（３８）のように
まとめて表す。このとき第１のセンサ及び第２のセンサ
の姿勢にずれがあり、各センサの観測諸元である距離、
仰角、方位角のそれぞれに式（３９）のバイアス誤差
（姿勢のバイアス誤差）とランダム誤差が含まれるとす
る。便宜上、２つのセンサの姿勢のバイアス誤差を式
（４０）のようにまとめて表す。また、ランダム誤差を
除いた第１及び第２のセンサによる目標の観測ベクトル
を式（４１）とする。また、各センサから見たｋ番目の
目標の真の位置ベクトルを式（４２）とし、便宜上これ
を式（４３）のようにまとめて表す。ここで、式（４
２）をそれぞれ各センサの位置を原点とする直交座標に
変換すると式（４４）で表される。
【００４９】
【数１８】
【００５０】基準点に対する、第１のセンサ１及び第２
のセンサから見たｋ番目の目標の位置ベクトルは、セン
サ姿勢及び位置のバイアス誤差、及びランダム誤差がな
ければ等しいので、各センサの真の位置ベクトルと、各
センサから見たｋ番目の目標の真の位置ベクトルの間に
は図２に示すように、式（４５）の関係がある。ここ
で、式（４５）の左辺と右辺の差をとった関数ｆを式
（４６）のように定義する。また、関数ｆを式（４７）
とおくと、式（３５）及び式（４４）より式（４８）で
ある。
のセンサから見たｋ番目の目標の位置ベクトルは、セン
サ姿勢及び位置のバイアス誤差、及びランダム誤差がな
ければ等しいので、各センサの真の位置ベクトルと、各
センサから見たｋ番目の目標の真の位置ベクトルの間に
は図２に示すように、式（４５）の関係がある。ここ
で、式（４５）の左辺と右辺の差をとった関数ｆを式
（４６）のように定義する。また、関数ｆを式（４７）
とおくと、式（３５）及び式（４４）より式（４８）で
ある。
【００５１】
【数１９】
【００５２】第１及び第２のセンサの姿勢及び位置のバ
イアス誤差の仮の値として式（４９）及び式（５０）を
式（４６）の関数ｆの平衡点の近傍に選んで、関数ｆを
式（５１）のまわりでテイラー展開し、２次以上の項を
無視すると、式（５２）が得られる。ここで、Ｌs
（ｔ），Ｍs （ｔ），Ｇk （ｔ），Ｊk （ｔ）は式（４
７）の偏微分結果より、式（５１）における各々式（５
３）、式（５４）、式（５５）、式（５６）の値であ
る。ただし、式（５１）における式（５３）と式（５
４）の値、及び、式（５５）と式（５６）の値は等しい
ので、式（５７）とおくと、式（５２）は式（５８）の
ように整理できる。
イアス誤差の仮の値として式（４９）及び式（５０）を
式（４６）の関数ｆの平衡点の近傍に選んで、関数ｆを
式（５１）のまわりでテイラー展開し、２次以上の項を
無視すると、式（５２）が得られる。ここで、Ｌs
（ｔ），Ｍs （ｔ），Ｇk （ｔ），Ｊk （ｔ）は式（４
７）の偏微分結果より、式（５１）における各々式（５
３）、式（５４）、式（５５）、式（５６）の値であ
る。ただし、式（５１）における式（５３）と式（５
４）の値、及び、式（５５）と式（５６）の値は等しい
ので、式（５７）とおくと、式（５２）は式（５８）の
ように整理できる。
【００５３】
【数２０】
【００５４】式（５８）の左辺は、各センサの姿勢及び
位置のバイアス誤差の初期値と、各センサの位置計測値
と、各センサによる目標の観測値とにより定まる観測値
と考えられる。式（５８）の右辺第１項及び第２項は求
めるべきセンサ姿勢及び位置のバイアス誤差を状態変数
として、観測行列との積と考えられる。式（５８）の右
辺第３項、第４項はそれぞれ各センサの位置計測値に含
まれるランダム誤差と観測行列の積、各センサによる目
標の観測値に含まれるランダム誤差と観測行列の積と考
えられる。従って、式（５８）は観測系に観測誤差が含
まれる線形状態方程式と考えることができる。即ち、観
測値をｚk（ｔ）アンダーバー，観測行列をＨk（ｔ），
求める状態変数をｘアンダーバー，観測雑音ベクトルを
νk（ｔ）アンダーバーとおけば、式（５８）は式（５
９）に置きかえられる。ここで、各項は式（６０）〜式
（６３）で表される。
位置のバイアス誤差の初期値と、各センサの位置計測値
と、各センサによる目標の観測値とにより定まる観測値
と考えられる。式（５８）の右辺第１項及び第２項は求
めるべきセンサ姿勢及び位置のバイアス誤差を状態変数
として、観測行列との積と考えられる。式（５８）の右
辺第３項、第４項はそれぞれ各センサの位置計測値に含
まれるランダム誤差と観測行列の積、各センサによる目
標の観測値に含まれるランダム誤差と観測行列の積と考
えられる。従って、式（５８）は観測系に観測誤差が含
まれる線形状態方程式と考えることができる。即ち、観
測値をｚk（ｔ）アンダーバー，観測行列をＨk（ｔ），
求める状態変数をｘアンダーバー，観測雑音ベクトルを
νk（ｔ）アンダーバーとおけば、式（５８）は式（５
９）に置きかえられる。ここで、各項は式（６０）〜式
（６３）で表される。
【００５５】
【数２１】
【００５６】観測値をｚk（ｔ）アンダーバーと状態変
数ｘアンダーバーとの差の自乗を評価関数にすることに
よって、最小自乗法により、センサ姿勢及び位置のバイ
アス誤差を推定することができる。センサ姿勢及び位置
のバイアス誤差の推定結果をｘt アンダーバーハットと
おけば、式（６４）となる。ここで、式（６５）及び式
（６６）で、Ｒ1s（ｔ）及びＲ2s（ｔ）はセンサ位置計
測器５でそれぞれ第１のセンサ１及び第のセンサ２の位
置を計測したときの計測誤差（ランダム誤差）の共分散
行列であり、Ｒ1k（ｔ）及びＲ2k（ｔ）はそれぞれ第１
及び第２のセンサで目標を観測したときの観測誤差（ラ
ンダム誤差）の共分散行列である。
数ｘアンダーバーとの差の自乗を評価関数にすることに
よって、最小自乗法により、センサ姿勢及び位置のバイ
アス誤差を推定することができる。センサ姿勢及び位置
のバイアス誤差の推定結果をｘt アンダーバーハットと
おけば、式（６４）となる。ここで、式（６５）及び式
（６６）で、Ｒ1s（ｔ）及びＲ2s（ｔ）はセンサ位置計
測器５でそれぞれ第１のセンサ１及び第のセンサ２の位
置を計測したときの計測誤差（ランダム誤差）の共分散
行列であり、Ｒ1k（ｔ）及びＲ2k（ｔ）はそれぞれ第１
及び第２のセンサで目標を観測したときの観測誤差（ラ
ンダム誤差）の共分散行列である。
【００５７】
【数２２】
【００５８】次に、上記に説明した原理を適用した実施
の形態１の動作について図１を参照して説明する。図１
において、第１のセンサ１及び第２のセンサ２から目標
の位置を観測し、第１の観測器３及び第２の観測器４に
より式（３７）で表される目標位置を観測し、各センサ
を原点とする直交座標に変換し、出力する処理までは従
来の装置と同一である。センサ位置計測器５では、第１
のセンサ及び第２のセンサ２の位置を式（２９）で表さ
れる基準直交座標にて計測する。第１の加算器６及び第
２の加算器７では、第１の観測器３及び第２の観測器４
からの目標の位置に、それぞれセンサ位置計測器５から
の第１のセンサの位置及び第２のセンサの位置を加算
し、基準点に対する目標の観測位置を算出する。センサ
姿勢及び位置の初期バイアス誤差設定器８は、第１のセ
ンサ１及び第２のセンサ２の姿勢及び位置のバイアス誤
差初期値を設定する。観測行列生成器９では、上記第１
の加算器６及び第２の加算器７からの目標の観測位置
と、センサ姿勢及び位置の初期バイアス誤差設定器８か
らのバイアス誤差初期値を受けて式（６１）で表される
観測行列を演算して求める。センサ姿勢及び位置のバイ
アス誤差推定値算出器１１では、センサ姿勢及び位置の
バイアス誤差推定値の仮の値、即ち式（６４）の右辺第
２項を算出する。さらに減算器１２では、センサ姿勢及
び位置のバイアス誤差推定値算出器１１からのセンサ姿
勢及び位置のバイアス誤差推定値の仮の値をセンサ姿勢
及び位置の初期バイアス誤差設定器８からのバイアス誤
差初期値、即ち式（６４）の右辺第１項から差し引い
て、式（６４）のセンサ姿勢及び位置のバイアス誤差推
定値を算出し出力する。
の形態１の動作について図１を参照して説明する。図１
において、第１のセンサ１及び第２のセンサ２から目標
の位置を観測し、第１の観測器３及び第２の観測器４に
より式（３７）で表される目標位置を観測し、各センサ
を原点とする直交座標に変換し、出力する処理までは従
来の装置と同一である。センサ位置計測器５では、第１
のセンサ及び第２のセンサ２の位置を式（２９）で表さ
れる基準直交座標にて計測する。第１の加算器６及び第
２の加算器７では、第１の観測器３及び第２の観測器４
からの目標の位置に、それぞれセンサ位置計測器５から
の第１のセンサの位置及び第２のセンサの位置を加算
し、基準点に対する目標の観測位置を算出する。センサ
姿勢及び位置の初期バイアス誤差設定器８は、第１のセ
ンサ１及び第２のセンサ２の姿勢及び位置のバイアス誤
差初期値を設定する。観測行列生成器９では、上記第１
の加算器６及び第２の加算器７からの目標の観測位置
と、センサ姿勢及び位置の初期バイアス誤差設定器８か
らのバイアス誤差初期値を受けて式（６１）で表される
観測行列を演算して求める。センサ姿勢及び位置のバイ
アス誤差推定値算出器１１では、センサ姿勢及び位置の
バイアス誤差推定値の仮の値、即ち式（６４）の右辺第
２項を算出する。さらに減算器１２では、センサ姿勢及
び位置のバイアス誤差推定値算出器１１からのセンサ姿
勢及び位置のバイアス誤差推定値の仮の値をセンサ姿勢
及び位置の初期バイアス誤差設定器８からのバイアス誤
差初期値、即ち式（６４）の右辺第１項から差し引い
て、式（６４）のセンサ姿勢及び位置のバイアス誤差推
定値を算出し出力する。
【００５９】このように、第１のセンサ及び第２のセン
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。
【００６０】実施の形態２．図３はこの発明の実施の形
態２を示すセンサ姿勢及び位置のバイアス誤差推定装置
の構成図である。図において、１，２，６，７は従来の
装置と同一である。また、８，１１，１２は実施の形態
１で用いられた要素と同じである。１４は第１のセンサ
１と第２のセンサ２の設置されている場所の地球の半径
を設定する曲率設定器、１５は第１のセンサ１からの目
標位置と、曲率設定器１４からの第１のセンサ１の設置
されている場所の地球の半径を用いて、基平面座標にお
ける目標位置を観測する第１の観測器、１６は第２のセ
ンサ２からの目標位置と、曲率設定器１４からの第２の
センサ２の設置されている場所の地球の半径を用いて、
基平面座標における目標位置を観測する第２の観測器、
１７は第１及び第２のセンサの位置を基平面座標で計測
するセンサ位置計測器、１８は第１の加算器６及び第２
の加算器７からの基平面座標における目標観測位置と、
センサ姿勢及び位置の初期バイアス誤差設定器８からの
センサ姿勢及び位置のバイアス誤差初期値とから目標の
観測行列を算出する観測行列生成器である。また、図４
は基平面座標におけるセンサ，目標の高度を説明する図
である。図示するように、センサ，目標の高さは海面か
らの高さｈs，ｈkにより表す。
態２を示すセンサ姿勢及び位置のバイアス誤差推定装置
の構成図である。図において、１，２，６，７は従来の
装置と同一である。また、８，１１，１２は実施の形態
１で用いられた要素と同じである。１４は第１のセンサ
１と第２のセンサ２の設置されている場所の地球の半径
を設定する曲率設定器、１５は第１のセンサ１からの目
標位置と、曲率設定器１４からの第１のセンサ１の設置
されている場所の地球の半径を用いて、基平面座標にお
ける目標位置を観測する第１の観測器、１６は第２のセ
ンサ２からの目標位置と、曲率設定器１４からの第２の
センサ２の設置されている場所の地球の半径を用いて、
基平面座標における目標位置を観測する第２の観測器、
１７は第１及び第２のセンサの位置を基平面座標で計測
するセンサ位置計測器、１８は第１の加算器６及び第２
の加算器７からの基平面座標における目標観測位置と、
センサ姿勢及び位置の初期バイアス誤差設定器８からの
センサ姿勢及び位置のバイアス誤差初期値とから目標の
観測行列を算出する観測行列生成器である。また、図４
は基平面座標におけるセンサ，目標の高度を説明する図
である。図示するように、センサ，目標の高さは海面か
らの高さｈs，ｈkにより表す。
【００６１】次に原理について説明する。図２はセン
サ、目標、地球の中心を通る平面で地球を切った断面図
により、地球の半径Ｒe，センサの海面高度ｈs，目標の
海面高度ｈk，センサの観測諸元である目標距離Ｒ，目
標仰角Ｅの関係を示している。この実施の形態２では、
実施の形態１で使用した基準直交座標に代わって、基準
直交座標におけるｘ座標、ｙ座標、及び海面からの高さ
ｈs，ｈkにより、センサ、目標の位置を表すことによ
り、地球の丸みに起因するセンサ及び目標の位置関係の
誤差の影響を軽減する。この座標系を基平面座標と呼
ぶ。ある基準点に対するセンサの位置を基平面座標で計
測するセンサ位置計測器１７によって時刻ｔに計測され
た第１のセンサ及び第２のセンサ２の位置ベクトル（位
置計測ベクトル）を式（６７）とし、便宜上これを式
（６８）のようにまとめて表す。ここで、センサ位置計
測器１７による第１のセンサ１及び第２のセンサ２の位
置の計測値に式（６９）のバイアス誤差（位置のバイア
ス誤差）とランダム誤差が含まれているとし、便宜上、
２つのセンサの位置のバイアス誤差を式（７０）のよう
にまとめて表す。また、第１のセンサ１及び第２のセン
サ２の位置計測ベクトルを式（７１）とし、各センサの
真の位置ベクトルを式（７２）で表すと、式（７３）の
関係がある。ここで、便宜上式（７２）を式（７４）の
ようにまとめて表す。また、ＡT はベクトルＡの転置ベ
クトルを示す。
サ、目標、地球の中心を通る平面で地球を切った断面図
により、地球の半径Ｒe，センサの海面高度ｈs，目標の
海面高度ｈk，センサの観測諸元である目標距離Ｒ，目
標仰角Ｅの関係を示している。この実施の形態２では、
実施の形態１で使用した基準直交座標に代わって、基準
直交座標におけるｘ座標、ｙ座標、及び海面からの高さ
ｈs，ｈkにより、センサ、目標の位置を表すことによ
り、地球の丸みに起因するセンサ及び目標の位置関係の
誤差の影響を軽減する。この座標系を基平面座標と呼
ぶ。ある基準点に対するセンサの位置を基平面座標で計
測するセンサ位置計測器１７によって時刻ｔに計測され
た第１のセンサ及び第２のセンサ２の位置ベクトル（位
置計測ベクトル）を式（６７）とし、便宜上これを式
（６８）のようにまとめて表す。ここで、センサ位置計
測器１７による第１のセンサ１及び第２のセンサ２の位
置の計測値に式（６９）のバイアス誤差（位置のバイア
ス誤差）とランダム誤差が含まれているとし、便宜上、
２つのセンサの位置のバイアス誤差を式（７０）のよう
にまとめて表す。また、第１のセンサ１及び第２のセン
サ２の位置計測ベクトルを式（７１）とし、各センサの
真の位置ベクトルを式（７２）で表すと、式（７３）の
関係がある。ここで、便宜上式（７２）を式（７４）の
ようにまとめて表す。また、ＡT はベクトルＡの転置ベ
クトルを示す。
【００６２】
【数２３】
【００６３】一方、第１のセンサ１及び第２のセンサ２
からｋ個の目標を観測して得られる目標の位置ベクトル
（以下、観測ベクトルと呼ぶ）はそれぞれ各センサを原
点とする極座標（距離、仰角、方位角）で表される。第
１のセンサ１及び第２のセンサ２による時刻ｔにおける
ｋ番目の目標の観測ベクトルを式（３７）とし、便宜上
これを式（３８）のようにまとめて表す。このとき第１
のセンサ及び第２のセンサの姿勢にずれがあり、各セン
サの観測諸元である距離、仰角、方位角のそれぞれに式
（３９）のバイアス誤差（姿勢のバイアス誤差）とラン
ダム誤差が含まれるとする。便宜上、２つのセンサの姿
勢のバイアス誤差を式（４０）のようにまとめて表す。
また、ランダム誤差を除いた第１のセンサ１及び第２の
センサ２による目標の観測ベクトルを式（４１）とす
る。また、各センサから見たｋ番目の目標の真の位置ベ
クトルを式（４２）とし、便宜上これを式（４３）のよ
うにまとめて表す。
からｋ個の目標を観測して得られる目標の位置ベクトル
（以下、観測ベクトルと呼ぶ）はそれぞれ各センサを原
点とする極座標（距離、仰角、方位角）で表される。第
１のセンサ１及び第２のセンサ２による時刻ｔにおける
ｋ番目の目標の観測ベクトルを式（３７）とし、便宜上
これを式（３８）のようにまとめて表す。このとき第１
のセンサ及び第２のセンサの姿勢にずれがあり、各セン
サの観測諸元である距離、仰角、方位角のそれぞれに式
（３９）のバイアス誤差（姿勢のバイアス誤差）とラン
ダム誤差が含まれるとする。便宜上、２つのセンサの姿
勢のバイアス誤差を式（４０）のようにまとめて表す。
また、ランダム誤差を除いた第１のセンサ１及び第２の
センサ２による目標の観測ベクトルを式（４１）とす
る。また、各センサから見たｋ番目の目標の真の位置ベ
クトルを式（４２）とし、便宜上これを式（４３）のよ
うにまとめて表す。
【００６４】図４において、センサ、目標、地球の中心
の３点よりなる三角形に余弦定理を使用すると式（７
５）を得る。式（７５）を展開し、ｈk 2及びｈs 2が２Ｒ
eに比べて十分に小さい、即ち式（７６）が成り立つと
すれば、式（７７）を得る。従って、基平面座標と各セ
ンサの観測諸元である距離、仰角、方位角（曲座標）の
間には式（７８）のような関係を近似できる。さらに、
センサの位置を基準とした基平面座標を式（７９）で定
義する。式（７９）に従って、式（４２）を各センサの
位置を基準とする基平面座標に変換し、式（３９）及び
式（４１）を使って表すと式（８０）の関係がある。
の３点よりなる三角形に余弦定理を使用すると式（７
５）を得る。式（７５）を展開し、ｈk 2及びｈs 2が２Ｒ
eに比べて十分に小さい、即ち式（７６）が成り立つと
すれば、式（７７）を得る。従って、基平面座標と各セ
ンサの観測諸元である距離、仰角、方位角（曲座標）の
間には式（７８）のような関係を近似できる。さらに、
センサの位置を基準とした基平面座標を式（７９）で定
義する。式（７９）に従って、式（４２）を各センサの
位置を基準とする基平面座標に変換し、式（３９）及び
式（４１）を使って表すと式（８０）の関係がある。
【００６５】
【数２４】
【００６６】実施の形態１で説明した図２の関係と同様
に、基平面座標における第１のセンサ１及び第２のセン
サから見たｋ番目の目標の位置ベクトルは、センサ姿勢
及び位置のバイアス誤差、及びランダム誤差がなければ
等しいので、各センサの真の位置ベクトルと、各センサ
から見たｋ番目の目標の真の位置ベクトルの間には式
（８１）の関係がある。ここで、式（８１）の左辺と右
辺の差をとった関数ｆを式（８２）のように定義する。
また、関数ｆを式（８３）とおくと、式（７３）及び式
（８０）より式（８４）である。
に、基平面座標における第１のセンサ１及び第２のセン
サから見たｋ番目の目標の位置ベクトルは、センサ姿勢
及び位置のバイアス誤差、及びランダム誤差がなければ
等しいので、各センサの真の位置ベクトルと、各センサ
から見たｋ番目の目標の真の位置ベクトルの間には式
（８１）の関係がある。ここで、式（８１）の左辺と右
辺の差をとった関数ｆを式（８２）のように定義する。
また、関数ｆを式（８３）とおくと、式（７３）及び式
（８０）より式（８４）である。
【００６７】
【数２５】
【００６８】第１のセンサと第２のセンサの姿勢及び位
置のバイアス誤差の仮の値として式（８５）及び式（８
６）を式（８２）の関数ｆの平衡点の近傍に選んで、関
数ｆを式（８７）のまわりでテイラー展開し、２次以上
の項を無視すると、式（８８）が得られる。ここで、Ｌ
s（ｔ）ブレーブ，Ｍs（ｔ）ブレーブ，Ｇk（ｔ）ブレ
ーブ，Ｊk（ｔ）ブレーブは式（８３）の偏微分結果よ
り、式（８７）における各々式（８９）、式（９０）、
式（９１）、式（９２）の値である。ただし、式（８
７）における式（８９）と式（９０）の値、及び、式
（９１）と式（９２）の値は等しいので、式（９３）と
おくと、式（８８）は式（９４）のように整理できる。
置のバイアス誤差の仮の値として式（８５）及び式（８
６）を式（８２）の関数ｆの平衡点の近傍に選んで、関
数ｆを式（８７）のまわりでテイラー展開し、２次以上
の項を無視すると、式（８８）が得られる。ここで、Ｌ
s（ｔ）ブレーブ，Ｍs（ｔ）ブレーブ，Ｇk（ｔ）ブレ
ーブ，Ｊk（ｔ）ブレーブは式（８３）の偏微分結果よ
り、式（８７）における各々式（８９）、式（９０）、
式（９１）、式（９２）の値である。ただし、式（８
７）における式（８９）と式（９０）の値、及び、式
（９１）と式（９２）の値は等しいので、式（９３）と
おくと、式（８８）は式（９４）のように整理できる。
【００６９】
【数２６】
【００７０】式（９４）の左辺は、各センサの姿勢及び
位置のバイアス誤差の初期値と、各センサの位置計測値
と、各センサによる目標の観測値とにより定まる観測値
と考えられる。式（９４）の右辺第１項及び第２項は求
めるべきセンサ姿勢及び位置のバイアス誤差を状態変数
として、観測行列との積と考えられる。式（９４）の右
辺第３項、第４項はそれぞれ各センサの位置計測値に含
まれるランダム誤差と観測行列の積、各センサによる目
標の観測値に含まれるランダム誤差と観測行列の積と考
えられる。従って、式（９４）は観測系に観測誤差が含
まれる線形状態方程式と考えることができる。即ち、観
測値をｚk（ｔ）アンダーバーブレーブ，観測行列をＨk
（ｔ）ブレーブ，求める状態変数をｘアンダーバーブレ
ーブ，観測雑音ベクトルをνk（ｔ）アンダーバーブレ
ーブとおけば、式（９４）は式（９５）に置きかえられ
る。ここで、各項は式（９６）〜式（９９）で表され
る。
位置のバイアス誤差の初期値と、各センサの位置計測値
と、各センサによる目標の観測値とにより定まる観測値
と考えられる。式（９４）の右辺第１項及び第２項は求
めるべきセンサ姿勢及び位置のバイアス誤差を状態変数
として、観測行列との積と考えられる。式（９４）の右
辺第３項、第４項はそれぞれ各センサの位置計測値に含
まれるランダム誤差と観測行列の積、各センサによる目
標の観測値に含まれるランダム誤差と観測行列の積と考
えられる。従って、式（９４）は観測系に観測誤差が含
まれる線形状態方程式と考えることができる。即ち、観
測値をｚk（ｔ）アンダーバーブレーブ，観測行列をＨk
（ｔ）ブレーブ，求める状態変数をｘアンダーバーブレ
ーブ，観測雑音ベクトルをνk（ｔ）アンダーバーブレ
ーブとおけば、式（９４）は式（９５）に置きかえられ
る。ここで、各項は式（９６）〜式（９９）で表され
る。
【００７１】
【数２７】
【００７２】観測値をｚk（ｔ）アンダーバーブレーブ
と状態変数ｘアンダーバーとの差の自乗を評価関数にす
ることによって、最小自乗法により、センサ姿勢及び位
置のバイアス誤差を推定することができる。センサ姿勢
及び位置のバイアス誤差の推定結果をｘt アンダーバー
ハットとおけば、式（１００）となる。ここで、式（１
０１）及び式（１０２）であり、Ｒ1s（ｔ）ブレーブ及
びＲ2s（ｔ）ブレーブはセンサ位置計測器１７でそれぞ
れ第１のセンサ１及び第のセンサ２の位置を計測したと
きの計測誤差（ランダム誤差）の共分散行列、Ｒ
1k（ｔ）及びＲ2k（ｔ）はそれぞれ第１のセンサ１及び
第２のセンサ２で目標を観測したときの観測誤差（ラン
ダム誤差）の共分散行列である。
と状態変数ｘアンダーバーとの差の自乗を評価関数にす
ることによって、最小自乗法により、センサ姿勢及び位
置のバイアス誤差を推定することができる。センサ姿勢
及び位置のバイアス誤差の推定結果をｘt アンダーバー
ハットとおけば、式（１００）となる。ここで、式（１
０１）及び式（１０２）であり、Ｒ1s（ｔ）ブレーブ及
びＲ2s（ｔ）ブレーブはセンサ位置計測器１７でそれぞ
れ第１のセンサ１及び第のセンサ２の位置を計測したと
きの計測誤差（ランダム誤差）の共分散行列、Ｒ
1k（ｔ）及びＲ2k（ｔ）はそれぞれ第１のセンサ１及び
第２のセンサ２で目標を観測したときの観測誤差（ラン
ダム誤差）の共分散行列である。
【００７３】
【数２８】
【００７４】次に、上記に説明した原理を適用した実施
の形態２の動作について図３を参照して説明する。図３
において、第１のセンサ１及び第２のセンサ２は目標の
位置を観測するためのセンサで、同時に目標位置をサン
プルする。曲率設定器１４は第１のセンサ１と第２のセ
ンサ２の設置されている場所の地球の半径を設定する。
第１の観測器１５及び第２の観測器１６は、曲率設定器
１４からの地球の半径を用いて、式（３７）で表される
目標の位置を観測し、式（７９）で表される各センサを
基準とする基平面座標に変換し出力する。センサ位置計
測器１７では、第１のセンサ及び第２のセンサ２の位置
を式（６７）で表される基平面座標にて計測する。第１
の加算器６及び第２の加算器７では、第１の観測器１５
及び第２の観測器１６からの目標の位置に、それぞれセ
ンサ位置計測器１７からの第１のセンサの位置及び第２
のセンサの位置を加算し、基準点に対する目標の観測位
置を算出する。センサ姿勢及び位置の初期バイアス誤差
設定器８は第１のセンサ１及び第２のセンサ２の姿勢及
び位置のバイアス誤差初期値を設定する。観測行列生成
器１８では、上記第１の加算器６及び第２の加算器７か
らの目標の観測位置と、センサ姿勢及び位置の初期バイ
アス誤差設定器８からのバイアス誤差初期値を受けて式
（９７）で表される観測行列を演算して求める。センサ
姿勢及び位置のバイアス誤差推定値算出器１１ではセン
サ姿勢及び位置のバイアス誤差推定値の仮の値、即ち式
（１００）の右辺第２項を算出する。さらに減算器１２
でセンサ姿勢及び位置のバイアス誤差推定値算出器１１
からのバイアス誤差推定値の仮の値をセンサ姿勢及び位
置の初期バイアス誤差設定器８からのバイアス誤差初期
値、即ち式（１００）の右辺第１項から差し引いて、式
（１００）のセンサ姿勢及び位置のバイアス誤差推定値
を算出し出力する。
の形態２の動作について図３を参照して説明する。図３
において、第１のセンサ１及び第２のセンサ２は目標の
位置を観測するためのセンサで、同時に目標位置をサン
プルする。曲率設定器１４は第１のセンサ１と第２のセ
ンサ２の設置されている場所の地球の半径を設定する。
第１の観測器１５及び第２の観測器１６は、曲率設定器
１４からの地球の半径を用いて、式（３７）で表される
目標の位置を観測し、式（７９）で表される各センサを
基準とする基平面座標に変換し出力する。センサ位置計
測器１７では、第１のセンサ及び第２のセンサ２の位置
を式（６７）で表される基平面座標にて計測する。第１
の加算器６及び第２の加算器７では、第１の観測器１５
及び第２の観測器１６からの目標の位置に、それぞれセ
ンサ位置計測器１７からの第１のセンサの位置及び第２
のセンサの位置を加算し、基準点に対する目標の観測位
置を算出する。センサ姿勢及び位置の初期バイアス誤差
設定器８は第１のセンサ１及び第２のセンサ２の姿勢及
び位置のバイアス誤差初期値を設定する。観測行列生成
器１８では、上記第１の加算器６及び第２の加算器７か
らの目標の観測位置と、センサ姿勢及び位置の初期バイ
アス誤差設定器８からのバイアス誤差初期値を受けて式
（９７）で表される観測行列を演算して求める。センサ
姿勢及び位置のバイアス誤差推定値算出器１１ではセン
サ姿勢及び位置のバイアス誤差推定値の仮の値、即ち式
（１００）の右辺第２項を算出する。さらに減算器１２
でセンサ姿勢及び位置のバイアス誤差推定値算出器１１
からのバイアス誤差推定値の仮の値をセンサ姿勢及び位
置の初期バイアス誤差設定器８からのバイアス誤差初期
値、即ち式（１００）の右辺第１項から差し引いて、式
（１００）のセンサ姿勢及び位置のバイアス誤差推定値
を算出し出力する。
【００７５】このように、第１のセンサ及び第２のセン
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。さらに、基準直交座標におけるｘ座標、ｙ座
標、及び海面からの高さを用いた基平面座標を用いて線
形状態方程式をたて、地球の丸みに起因するセンサ及び
目標の位置関係の誤差の影響を軽減しているので、各セ
ンサの位置のバイアス誤差及び姿勢のバイアス誤差の推
定精度がよい。
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。さらに、基準直交座標におけるｘ座標、ｙ座
標、及び海面からの高さを用いた基平面座標を用いて線
形状態方程式をたて、地球の丸みに起因するセンサ及び
目標の位置関係の誤差の影響を軽減しているので、各セ
ンサの位置のバイアス誤差及び姿勢のバイアス誤差の推
定精度がよい。
【００７６】実施の形態３．図５はこの発明の実施の形
態３のセンサ姿勢及び位置のバイアス誤差推定装置の構
成図である。図において、１〜９，１１〜１２は実施の
形態１の各要素と同一である。１０は観測行列から第１
のセンサ１及び第２のセンサ２の姿勢及び位置のバイア
ス誤差推定値の共分散行列を算出するセンサ姿勢及び位
置のバイアス誤差推定値評価器、１９は減算器１２から
の時刻ｔにおける第１のセンサと第２のセンサのバイア
ス誤差推定値を記憶しておくセンサ姿勢及び位置のバイ
アス誤差推定値記憶器である。
態３のセンサ姿勢及び位置のバイアス誤差推定装置の構
成図である。図において、１〜９，１１〜１２は実施の
形態１の各要素と同一である。１０は観測行列から第１
のセンサ１及び第２のセンサ２の姿勢及び位置のバイア
ス誤差推定値の共分散行列を算出するセンサ姿勢及び位
置のバイアス誤差推定値評価器、１９は減算器１２から
の時刻ｔにおける第１のセンサと第２のセンサのバイア
ス誤差推定値を記憶しておくセンサ姿勢及び位置のバイ
アス誤差推定値記憶器である。
【００７７】次に原理について説明する。第１のセンサ
及び第２のセンサから目標の位置を観測し、そのデータ
から観測行列生成器９で目標の観測行列を生成する処理
までは実施の形態１と同一である。式（５９）の状態ベ
クトルはセンサ姿勢及び位置のバイアス誤差で、時刻に
よらない定数ベクトルなので、カルマンフィルタの理論
において推移行列を単位行列、駆動雑音を零ベクトルと
して、センサ姿勢及び位置のバイアス誤差を収束計算に
より推定することができる。センサ姿勢及び位置のバイ
アス誤差の推定結果をｘt アンダーバーハットとおけ
ば、式（１０３）となる。ここで、式（１０４）及び式
（１０８）で、Ｒ1s（ｔ）及びＲ2s（ｔ）はセンサ位置
計測器５でそれぞれ第１のセンサ１及び第のセンサ２の
位置を計測したときの計測誤差（ランダム誤差）の共分
散行列であり、Ｒ1k（ｔ）及びＲ2k（ｔ）はそれぞれ第
１のセンサ１及び第２のセンサ２で目標を観測したとき
の観測誤差（ランダム誤差）の共分散行列である。
及び第２のセンサから目標の位置を観測し、そのデータ
から観測行列生成器９で目標の観測行列を生成する処理
までは実施の形態１と同一である。式（５９）の状態ベ
クトルはセンサ姿勢及び位置のバイアス誤差で、時刻に
よらない定数ベクトルなので、カルマンフィルタの理論
において推移行列を単位行列、駆動雑音を零ベクトルと
して、センサ姿勢及び位置のバイアス誤差を収束計算に
より推定することができる。センサ姿勢及び位置のバイ
アス誤差の推定結果をｘt アンダーバーハットとおけ
ば、式（１０３）となる。ここで、式（１０４）及び式
（１０８）で、Ｒ1s（ｔ）及びＲ2s（ｔ）はセンサ位置
計測器５でそれぞれ第１のセンサ１及び第のセンサ２の
位置を計測したときの計測誤差（ランダム誤差）の共分
散行列であり、Ｒ1k（ｔ）及びＲ2k（ｔ）はそれぞれ第
１のセンサ１及び第２のセンサ２で目標を観測したとき
の観測誤差（ランダム誤差）の共分散行列である。
【００７８】
【数２９】
【００７９】次に、上記に説明した原理を適用した実施
の形態３の動作について図５を参照して説明する。図５
において、第１及び第２のセンサから目標の位置を観測
し、第１の観測器３及び第２の観測器４により式（３
７）で表される目標位置を観測し、第１の加算器６及び
第２の加算器７で、基準点に対する目標の観測位置を算
出する処理までは実施の形態１と同一である。センサ姿
勢及び位置の初期バイアス誤差設定器８は動作開始時
（ｔ＝１）では第１のセンサ１及び第２のセンサ２の姿
勢及び位置のバイアス誤差初期値を設定し、それ以後
（ｔ＞１）はセンサ姿勢及び位置のバイアス誤差推定値
記憶器１９からの前回算出のセンサ姿勢及び位置のバイ
アス誤差推定値を出力する。観測行列生成器９では、上
記第１の加算器６及び第２の加算器７からの目標の観測
位置と、センサ姿勢及び位置の初期バイアス誤差設定器
８からのバイアス誤差初期値または前回算出のセンサ姿
勢及び位置のバイアス誤差推定値を受けて式（６１）で
表される観測行列を演算して求める。センサ姿勢及び位
置のバイアス誤差推定値評価器１０では式（１０７）に
おける共分散行列を演算する。さらに、センサ姿勢及び
位置のバイアス誤差推定値算出器１１ではセンサ姿勢及
び位置のバイアス誤差推定値評価器１０からの推定値の
共分散行列を用いて、センサ姿勢及び位置のバイアス誤
差推定値の仮の値、即ち式（１０３）の右辺第２項を算
出する。さらに、減算器１２でセンサ姿勢及び位置のバ
イアス誤差推定値算出器１１からのセンサ姿勢及び位置
のバイアス誤差推定値の仮の値をセンサ姿勢及び位置の
初期バイアス誤差設定器８からのバイアス誤差初期値ま
たは前回算出のセンサ姿勢及び位置のバイアス誤差推定
値、即ち式（１０３）の右辺第１項から差し引いて、式
（１０３）のセンサ姿勢及び位置のバイアス誤差推定値
を算出し出力する。センサ姿勢及び位置のバイアス誤差
推定値記憶器１９では、算出したサンプリング時刻ｔに
おけるセンサ姿勢及び位置のバイアス誤差推定値を記憶
するとともに、前回算出のセンサ姿勢及び位置のバイア
ス誤差推定値としてセンサ姿勢及び位置の初期バイアス
設定器８に送出する。以後、この一連の処理をセンサ姿
勢及び位置のバイアス誤差推定値が収束するまで繰り返
す。
の形態３の動作について図５を参照して説明する。図５
において、第１及び第２のセンサから目標の位置を観測
し、第１の観測器３及び第２の観測器４により式（３
７）で表される目標位置を観測し、第１の加算器６及び
第２の加算器７で、基準点に対する目標の観測位置を算
出する処理までは実施の形態１と同一である。センサ姿
勢及び位置の初期バイアス誤差設定器８は動作開始時
（ｔ＝１）では第１のセンサ１及び第２のセンサ２の姿
勢及び位置のバイアス誤差初期値を設定し、それ以後
（ｔ＞１）はセンサ姿勢及び位置のバイアス誤差推定値
記憶器１９からの前回算出のセンサ姿勢及び位置のバイ
アス誤差推定値を出力する。観測行列生成器９では、上
記第１の加算器６及び第２の加算器７からの目標の観測
位置と、センサ姿勢及び位置の初期バイアス誤差設定器
８からのバイアス誤差初期値または前回算出のセンサ姿
勢及び位置のバイアス誤差推定値を受けて式（６１）で
表される観測行列を演算して求める。センサ姿勢及び位
置のバイアス誤差推定値評価器１０では式（１０７）に
おける共分散行列を演算する。さらに、センサ姿勢及び
位置のバイアス誤差推定値算出器１１ではセンサ姿勢及
び位置のバイアス誤差推定値評価器１０からの推定値の
共分散行列を用いて、センサ姿勢及び位置のバイアス誤
差推定値の仮の値、即ち式（１０３）の右辺第２項を算
出する。さらに、減算器１２でセンサ姿勢及び位置のバ
イアス誤差推定値算出器１１からのセンサ姿勢及び位置
のバイアス誤差推定値の仮の値をセンサ姿勢及び位置の
初期バイアス誤差設定器８からのバイアス誤差初期値ま
たは前回算出のセンサ姿勢及び位置のバイアス誤差推定
値、即ち式（１０３）の右辺第１項から差し引いて、式
（１０３）のセンサ姿勢及び位置のバイアス誤差推定値
を算出し出力する。センサ姿勢及び位置のバイアス誤差
推定値記憶器１９では、算出したサンプリング時刻ｔに
おけるセンサ姿勢及び位置のバイアス誤差推定値を記憶
するとともに、前回算出のセンサ姿勢及び位置のバイア
ス誤差推定値としてセンサ姿勢及び位置の初期バイアス
設定器８に送出する。以後、この一連の処理をセンサ姿
勢及び位置のバイアス誤差推定値が収束するまで繰り返
す。
【００８０】このように、第１のセンサ及び第２のセン
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。さらに、目標観測位置と前回算出したセンサ
姿勢及び位置のバイアス誤差推定値とから収束演算する
ので推定精度がよい。
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。さらに、目標観測位置と前回算出したセンサ
姿勢及び位置のバイアス誤差推定値とから収束演算する
ので推定精度がよい。
【００８１】実施の形態４．図６はこの発明の実施の形
態４を示すセンサ姿勢及び位置のバイアス誤差推定装置
の構成図である。図において、１，２，６〜８，１１，
１２，１４〜１８は実施の形態２の各要素と同一であ
る。１０は観測行列から第１のセンサ１及び第２のセン
サ２の姿勢及び位置のバイアス誤差推定値の共分散行列
を算出するセンサ姿勢及び位置のバイアス誤差推定値評
価器、１９は減算器１２からの時刻ｔにおける第１のセ
ンサとセンサのバイアス誤差推定値を記憶しておくセン
サ姿勢及び位置のバイアス誤差推定値記憶器である。
態４を示すセンサ姿勢及び位置のバイアス誤差推定装置
の構成図である。図において、１，２，６〜８，１１，
１２，１４〜１８は実施の形態２の各要素と同一であ
る。１０は観測行列から第１のセンサ１及び第２のセン
サ２の姿勢及び位置のバイアス誤差推定値の共分散行列
を算出するセンサ姿勢及び位置のバイアス誤差推定値評
価器、１９は減算器１２からの時刻ｔにおける第１のセ
ンサとセンサのバイアス誤差推定値を記憶しておくセン
サ姿勢及び位置のバイアス誤差推定値記憶器である。
【００８２】次に原理について説明する。第１のセンサ
及び第２のセンサから目標の位置を観測し、そのデータ
から観測行列生成器１８で目標の観測行列を生成する処
理までは実施の形態２と同一である。式（９５）の状態
ベクトルはセンサ姿勢及び位置のバイアス誤差で、時刻
によらない定数ベクトルなので、カルマンフィルタの理
論において推移行列を単位行列、駆動雑音を零ベクトル
として、センサ姿勢及び位置のバイアス誤差を推定する
ことができる。センサ姿勢及び位置のバイアス誤差の推
定結果をｘt アンダーバーハットとおけば、式（１０
９）となる。ここで、式（１１０）及び式（１１４）で
あり、Ｒ1s（ｔ）ブレーブ及びＲ2s（ｔ）ブレーブはセ
ンサ位置計測器１７でそれぞれ第１のセンサ１及び第の
センサ２の位置を計測したときの計測誤差（ランダム誤
差）の共分散行列、Ｒ1k（ｔ）及びＲ2k（ｔ）はそれぞ
れ第１のセンサ１及び第２のセンサ２で目標を観測した
ときの観測誤差（ランダム誤差）の共分散行列である。
及び第２のセンサから目標の位置を観測し、そのデータ
から観測行列生成器１８で目標の観測行列を生成する処
理までは実施の形態２と同一である。式（９５）の状態
ベクトルはセンサ姿勢及び位置のバイアス誤差で、時刻
によらない定数ベクトルなので、カルマンフィルタの理
論において推移行列を単位行列、駆動雑音を零ベクトル
として、センサ姿勢及び位置のバイアス誤差を推定する
ことができる。センサ姿勢及び位置のバイアス誤差の推
定結果をｘt アンダーバーハットとおけば、式（１０
９）となる。ここで、式（１１０）及び式（１１４）で
あり、Ｒ1s（ｔ）ブレーブ及びＲ2s（ｔ）ブレーブはセ
ンサ位置計測器１７でそれぞれ第１のセンサ１及び第の
センサ２の位置を計測したときの計測誤差（ランダム誤
差）の共分散行列、Ｒ1k（ｔ）及びＲ2k（ｔ）はそれぞ
れ第１のセンサ１及び第２のセンサ２で目標を観測した
ときの観測誤差（ランダム誤差）の共分散行列である。
【００８３】
【数３０】
【００８４】次に、上記に説明した原理を適用した実施
の形態４の動作について図６を参照して説明する。図６
において、第１及び第２のセンサから目標の位置を観測
し、第１の加算器６及び第２の加算器７で、基準点に対
する目標の観測位置を算出する処理までは実施の形態２
と同一である。センサ姿勢及び位置の初期バイアス誤差
設定器８は動作開始時（ｔ＝１）では第１のセンサ１及
び第２のセンサ２の姿勢及び位置のバイアス誤差初期値
を設定し、それ以後（ｔ＞１）はセンサ姿勢及び位置の
バイアス誤差推定値記憶器１９からの前回算出のセンサ
姿勢及び位置のバイアス誤差推定値を出力する。
の形態４の動作について図６を参照して説明する。図６
において、第１及び第２のセンサから目標の位置を観測
し、第１の加算器６及び第２の加算器７で、基準点に対
する目標の観測位置を算出する処理までは実施の形態２
と同一である。センサ姿勢及び位置の初期バイアス誤差
設定器８は動作開始時（ｔ＝１）では第１のセンサ１及
び第２のセンサ２の姿勢及び位置のバイアス誤差初期値
を設定し、それ以後（ｔ＞１）はセンサ姿勢及び位置の
バイアス誤差推定値記憶器１９からの前回算出のセンサ
姿勢及び位置のバイアス誤差推定値を出力する。
【００８５】観測行列生成器１８では、上記第１の加算
器６及び第２の加算器７からの目標の観測位置と、セン
サ姿勢及び位置の初期バイアス誤差設定器８からのバイ
アス誤差初期値または前回算出のセンサ姿勢及び位置の
バイアス誤差推定値を受けて式（９７）で表される観測
行列を演算して求める。センサ姿勢及び位置のバイアス
誤差推定値評価器１０では、式（１１３）における共分
散行列を演算する。さらに、センサ姿勢及び位置のバイ
アス誤差推定値算出器１１ではセンサ姿勢及び位置のバ
イアス誤差推定値評価器１０からの推定値の共分散行列
を用いて、センサ姿勢及び位置のバイアス誤差推定値の
仮の値、即ち式（１０９）の右辺第２項を算出する。さ
らに、減算器１２でセンサ姿勢及び位置のバイアス誤差
推定値算出器１１からのセンサ姿勢及び位置のバイアス
誤差推定値の仮の値をセンサ姿勢及び位置の初期バイア
ス誤差設定器８からのバイアス誤差初期値または前回算
出のセンサ姿勢及び位置のバイアス誤差推定値、即ち式
（１０９）の右辺第１項から差し引いて、式（１０９）
のセンサ姿勢及び位置のバイアス誤差推定値を算出し出
力する。センサ姿勢及び位置のバイアス誤差推定値記憶
器１９では、算出したサンプリング時刻ｔにおけるセン
サ姿勢及び位置のバイアス誤差推定値を記憶するととも
に、前回算出のセンサ姿勢及び位置のバイアス誤差推定
値としてセンサ姿勢及び位置の初期バイアス設定器８に
送出する。以後、この一連の処理をセンサ姿勢及び位置
のバイアス誤差推定値が収束するまで繰り返す。
器６及び第２の加算器７からの目標の観測位置と、セン
サ姿勢及び位置の初期バイアス誤差設定器８からのバイ
アス誤差初期値または前回算出のセンサ姿勢及び位置の
バイアス誤差推定値を受けて式（９７）で表される観測
行列を演算して求める。センサ姿勢及び位置のバイアス
誤差推定値評価器１０では、式（１１３）における共分
散行列を演算する。さらに、センサ姿勢及び位置のバイ
アス誤差推定値算出器１１ではセンサ姿勢及び位置のバ
イアス誤差推定値評価器１０からの推定値の共分散行列
を用いて、センサ姿勢及び位置のバイアス誤差推定値の
仮の値、即ち式（１０９）の右辺第２項を算出する。さ
らに、減算器１２でセンサ姿勢及び位置のバイアス誤差
推定値算出器１１からのセンサ姿勢及び位置のバイアス
誤差推定値の仮の値をセンサ姿勢及び位置の初期バイア
ス誤差設定器８からのバイアス誤差初期値または前回算
出のセンサ姿勢及び位置のバイアス誤差推定値、即ち式
（１０９）の右辺第１項から差し引いて、式（１０９）
のセンサ姿勢及び位置のバイアス誤差推定値を算出し出
力する。センサ姿勢及び位置のバイアス誤差推定値記憶
器１９では、算出したサンプリング時刻ｔにおけるセン
サ姿勢及び位置のバイアス誤差推定値を記憶するととも
に、前回算出のセンサ姿勢及び位置のバイアス誤差推定
値としてセンサ姿勢及び位置の初期バイアス設定器８に
送出する。以後、この一連の処理をセンサ姿勢及び位置
のバイアス誤差推定値が収束するまで繰り返す。
【００８６】このように、第１のセンサ及び第２のセン
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。さらに、基準直交座標におけるｘ座標、ｙ座
標、及び海面からの高さを用いた基平面座標を用いて線
形状態方程式をたて、地球の丸みに起因するセンサ及び
目標の位置関係の誤差の影響を軽減しているので、各セ
ンサの位置のバイアス誤差及び姿勢のバイアス誤差の推
定精度がよい。またさらに、目標観測位置と前回算出し
たセンサ姿勢及び位置のバイアス誤差推定値とから収束
演算するので推定精度がよい。
サの姿勢及び位置のバイアス誤差を状態変数とし、各セ
ンサの位置計測器及び各センサの観測系にランダム誤差
が含まれる線形状態方程式をたてて、バイアス誤差を推
定しているので、各センサの位置の計測値にバイアス誤
差及びランダム誤差が含まれる場合にも、各センサの姿
勢及び位置のバイアス誤差を同時に精度よく求めること
ができる。さらに、基準直交座標におけるｘ座標、ｙ座
標、及び海面からの高さを用いた基平面座標を用いて線
形状態方程式をたて、地球の丸みに起因するセンサ及び
目標の位置関係の誤差の影響を軽減しているので、各セ
ンサの位置のバイアス誤差及び姿勢のバイアス誤差の推
定精度がよい。またさらに、目標観測位置と前回算出し
たセンサ姿勢及び位置のバイアス誤差推定値とから収束
演算するので推定精度がよい。
【００８７】
【発明の効果】以上のように請求項１の発明によれば、
観測行列生成手段と、センサ姿勢及び位置のバイアス誤
差推定手段を設け、２つのセンサの姿勢及び位置のバイ
アス誤差を状態変数とした線形状態方程式の観測行列を
算出し、センサの姿勢及び位置のバイアス誤差を推定し
ているので、各センサの姿勢及び位置のバイアス誤差を
同時に精度よく求めることができるセンサ姿勢及び位置
のバイアス誤差推定装置を得ることができる。
観測行列生成手段と、センサ姿勢及び位置のバイアス誤
差推定手段を設け、２つのセンサの姿勢及び位置のバイ
アス誤差を状態変数とした線形状態方程式の観測行列を
算出し、センサの姿勢及び位置のバイアス誤差を推定し
ているので、各センサの姿勢及び位置のバイアス誤差を
同時に精度よく求めることができるセンサ姿勢及び位置
のバイアス誤差推定装置を得ることができる。
【００８８】また、この請求項２の発明によれば、観測
行列生成手段と、センサ姿勢及び位置のバイアス誤差推
定手段を設け、２つのセンサの姿勢及び位置のバイアス
誤差を状態変数とした線形状態方程式の観測行列を算出
し、センサの姿勢及び位置のバイアス誤差を推定してい
るので、各センサの姿勢及び位置のバイアス誤差を同時
に精度よく求めることができ、さらに、２つのセンサの
位置の地球の半径を設定する曲率設定器を設け、基準直
交座標におけるｘ座標、ｙ座標と海面からの高さを用い
た基平面座標を用いて線形状態方程式をたて、地球の丸
みに起因するセンサ及び目標の位置関係の誤差の影響を
軽減しているので、精度よくセンサ姿勢及び位置のバイ
アス誤差を推定できるセンサ姿勢及び位置のバイアス誤
差推定装置を得ることができる。
行列生成手段と、センサ姿勢及び位置のバイアス誤差推
定手段を設け、２つのセンサの姿勢及び位置のバイアス
誤差を状態変数とした線形状態方程式の観測行列を算出
し、センサの姿勢及び位置のバイアス誤差を推定してい
るので、各センサの姿勢及び位置のバイアス誤差を同時
に精度よく求めることができ、さらに、２つのセンサの
位置の地球の半径を設定する曲率設定器を設け、基準直
交座標におけるｘ座標、ｙ座標と海面からの高さを用い
た基平面座標を用いて線形状態方程式をたて、地球の丸
みに起因するセンサ及び目標の位置関係の誤差の影響を
軽減しているので、精度よくセンサ姿勢及び位置のバイ
アス誤差を推定できるセンサ姿勢及び位置のバイアス誤
差推定装置を得ることができる。
【００８９】また、この請求項３の発明によれば、観測
行列生成手段と、センサ姿勢及び位置のバイアス誤差推
定手段を設け、２つのセンサの姿勢及び位置のバイアス
誤差を状態変数とした線形状態方程式の観測行列を算出
し、センサの姿勢及び位置のバイアス誤差を推定してい
るので、各センサの姿勢及び位置のバイアス誤差を同時
に精度よく求めることができ、さらに、センサ姿勢及び
位置のバイアス誤差推定値評価手段を設け、センサの姿
勢及び位置のバイアス誤差推定値の共分散行列を算出
し、今回観測した目標位置と前回算出したセンサの姿勢
及び位置のバイアス誤差推定値とから収束演算をするの
で、推定精度がよいセンサ姿勢及び位置のバイアス誤差
推定装置を得ることができる。
行列生成手段と、センサ姿勢及び位置のバイアス誤差推
定手段を設け、２つのセンサの姿勢及び位置のバイアス
誤差を状態変数とした線形状態方程式の観測行列を算出
し、センサの姿勢及び位置のバイアス誤差を推定してい
るので、各センサの姿勢及び位置のバイアス誤差を同時
に精度よく求めることができ、さらに、センサ姿勢及び
位置のバイアス誤差推定値評価手段を設け、センサの姿
勢及び位置のバイアス誤差推定値の共分散行列を算出
し、今回観測した目標位置と前回算出したセンサの姿勢
及び位置のバイアス誤差推定値とから収束演算をするの
で、推定精度がよいセンサ姿勢及び位置のバイアス誤差
推定装置を得ることができる。
【００９０】また、この請求項４の発明によれば、観測
行列生成手段と、センサ姿勢及び位置のバイアス誤差推
定手段を設け、２つのセンサの姿勢及び位置のバイアス
誤差を状態変数とした線形状態方程式の観測行列を算出
し、センサの姿勢及び位置のバイアス誤差を推定してい
るので、各センサの姿勢及び位置のバイアス誤差を同時
に精度よく求めることができ、さらに、２つのセンサの
位置の地球の半径を設定する曲率設定器を設け、基準直
交座標におけるｘ座標、ｙ座標と海面からの高さを用い
た基平面座標を用いて線形状態方程式をたて、地球の丸
みに起因するセンサ及び目標の位置関係の誤差の影響を
軽減しているので、精度よくセンサ姿勢及び位置のバイ
アス誤差を推定でき、またさらに、センサ姿勢及び位置
のバイアス誤差推定値評価手段を設け、センサの姿勢及
び位置のバイアス誤差推定値の共分散行列を算出し、今
回観測した目標位置と前回算出したセンサの姿勢及び位
置のバイアス誤差推定値とから収束演算をするので、推
定精度がよい、センサ姿勢及び位置のバイアス誤差推定
装置を得ることができる。
行列生成手段と、センサ姿勢及び位置のバイアス誤差推
定手段を設け、２つのセンサの姿勢及び位置のバイアス
誤差を状態変数とした線形状態方程式の観測行列を算出
し、センサの姿勢及び位置のバイアス誤差を推定してい
るので、各センサの姿勢及び位置のバイアス誤差を同時
に精度よく求めることができ、さらに、２つのセンサの
位置の地球の半径を設定する曲率設定器を設け、基準直
交座標におけるｘ座標、ｙ座標と海面からの高さを用い
た基平面座標を用いて線形状態方程式をたて、地球の丸
みに起因するセンサ及び目標の位置関係の誤差の影響を
軽減しているので、精度よくセンサ姿勢及び位置のバイ
アス誤差を推定でき、またさらに、センサ姿勢及び位置
のバイアス誤差推定値評価手段を設け、センサの姿勢及
び位置のバイアス誤差推定値の共分散行列を算出し、今
回観測した目標位置と前回算出したセンサの姿勢及び位
置のバイアス誤差推定値とから収束演算をするので、推
定精度がよい、センサ姿勢及び位置のバイアス誤差推定
装置を得ることができる。
【図１】この発明の実施の形態１を示すセンサ姿勢及び
位置のバイアス誤差推定装置の構成図である。
位置のバイアス誤差推定装置の構成図である。
【図２】基準点に対する目標位置ベクトルを説明する図
である。
である。
【図３】この発明の実施の形態２を示すセンサ姿勢及び
位置のバイアス誤差推定装置の構成図である。
位置のバイアス誤差推定装置の構成図である。
【図４】基平面座標における目標の高度を説明する図で
ある。
ある。
【図５】この発明の実施の形態３を示すセンサ姿勢及び
位置のバイアス誤差推定装置の構成図である。
位置のバイアス誤差推定装置の構成図である。
【図６】この発明の実施の形態４を示すセンサ姿勢及び
位置のバイアス誤差推定装置の構成図である。
位置のバイアス誤差推定装置の構成図である。
【図７】従来のセンサ姿勢のバイアス誤差推定装置の構
成図である。
成図である。
１ 第１のセンサ、２ 第２のセンサ、３ 第１の観測
器、４ 第２の観測器、５ センサ位置計測器、６，７
加算器、８ センサ姿勢及び位置の初期バイアス誤差
設定器、９ 観測行列生成器、１０ センサ姿勢及び位
置のバイアス誤差推定値評価器、１１ センサ姿勢及び
位置のバイアス誤差推定値算出器、１２減算器、１４
曲率設定器、１５ 第１の観測器、１６ 第２の観測
器、１７センサ位置計測器、１８ 観測行列生成器、１
９ センサ姿勢及び位置のバイアス誤差推定値記憶器、
２０ センサ位置設定器、２１ センサ姿勢の初期バイ
アス誤差設定器、２２ 観測行列生成器、２３ センサ
姿勢のバイアス誤差推定値評価器、２４ センサ姿勢の
バイアス誤差推定値算出器、２５ 減算器、２７センサ
姿勢のバイアス誤差記憶器。
器、４ 第２の観測器、５ センサ位置計測器、６，７
加算器、８ センサ姿勢及び位置の初期バイアス誤差
設定器、９ 観測行列生成器、１０ センサ姿勢及び位
置のバイアス誤差推定値評価器、１１ センサ姿勢及び
位置のバイアス誤差推定値算出器、１２減算器、１４
曲率設定器、１５ 第１の観測器、１６ 第２の観測
器、１７センサ位置計測器、１８ 観測行列生成器、１
９ センサ姿勢及び位置のバイアス誤差推定値記憶器、
２０ センサ位置設定器、２１ センサ姿勢の初期バイ
アス誤差設定器、２２ 観測行列生成器、２３ センサ
姿勢のバイアス誤差推定値評価器、２４ センサ姿勢の
バイアス誤差推定値算出器、２５ 減算器、２７センサ
姿勢のバイアス誤差記憶器。
Claims (4)
- 【請求項１】 互いに離れた位置にある２つのセンサか
ら観測器を介してｋ個の目標の位置を観測し各センサを
原点とする直交座標に変換した出力と、上記２つのセン
サの位置を基準直交座標にて計測したセンサ位置計測器
の出力とを加算して、上記基準直交座標にて上記目標の
観測位置を求めて一方の入力とし、 上記センサ自体に起因しその観測諸元である目標の距
離，仰角，方位角各々に含まれるバイアス誤差をセンサ
姿勢のバイアス誤差と定義し、上記センサ位置の計測値
に含まれるバイアス誤差をセンサ位置のバイアス誤差と
定義し、それらセンサ姿勢及び位置のバイアス誤差の初
期値を設定するセンサ姿勢及び位置の初期バイアス誤差
設定器の出力を他方の入力として、 それらセンサ姿勢及び位置のバイアス誤差を状態変数と
した線形状態方程式の観測行列を算出する観測行列生成
手段と、 上記観測行列からセンサ姿勢及び位置のバイアス誤差推
定値の仮の値を算出するセンサ姿勢及び位置のバイアス
誤差推定値算出手段と、 上記センサ姿勢及び位置のバイアス誤差の初期値から上
記センサ姿勢及び位置のバイアス誤差推定値の仮の値を
差し引いてセンサ姿勢及び位置のバイアス誤差推定値を
出力する手段と、 を備えたことを特徴とするセンサ姿勢及び位置のバイア
ス誤差推定装置。 - 【請求項２】 互いに離れた位置にある２つのセンサ位
置の地球の半径を設定する曲率設定器と、 上記２つのセンサから観測器を介してｋ個の目標の位置
を観測し各センサを原点とする直交座標のｘ座標及びｙ
座標と、海面からの高さを用いた基平面座標に変換した
出力と、上記２つのセンサの位置を基準の基平面座標に
て計測したセンサ位置計測器の出力とを加算して、上記
基準の基平面座標にて上記目標の観測位置を求めて一方
の入力とし、 上記センサ自体に起因しその観測諸元である目標の距
離，仰角，方位角各々に含まれるバイアス誤差をセンサ
姿勢のバイアス誤差と定義し、上記センサ位置の計測値
に含まれるバイアス誤差をセンサ位置のバイアス誤差と
定義し、それらセンサ姿勢及び位置のバイアス誤差の初
期値を設定するセンサ姿勢及び位置の初期バイアス誤差
設定器の出力を他方の入力として、 それらセンサ姿勢及び位置のバイアス誤差を状態変数と
した線形状態方程式の観測行列を算出する観測行列生成
手段と、 上記観測行列からセンサ姿勢及び位置のバイアス誤差推
定値の仮の値を算出するセンサ姿勢及び位置のバイアス
誤差推定値算出手段と、 上記センサ姿勢及び位置のバイアス誤差の初期値から上
記センサ姿勢及び位置のバイアス誤差推定値の仮の値を
差し引いてセンサ姿勢及び位置のバイアス誤差推定値を
出力する手段と、を備えたことを特徴とするセンサ姿勢
及び位置のバイアス誤差推定装置。 - 【請求項３】 互いに離れた位置にある２つのセンサか
ら観測器を介してｋ個の目標の位置を観測し各センサを
原点とする直交座標に変換した出力と、上記２つのセン
サの位置を基準直交座標にて計測したセンサ位置計測器
の出力とを加算して、上記基準直交座標にて上記目標の
観測位置を求めて一方の入力とし、 上記センサ自体に起因しその観測諸元である目標の距
離，仰角，方位角各々に含まれるバイアス誤差をセンサ
姿勢のバイアス誤差と定義し、上記センサ位置の計測値
に含まれるバイアス誤差をセンサ位置のバイアス誤差と
定義し、それらセンサ姿勢及び位置のバイアス誤差の初
期値を設定するセンサ姿勢及び位置の初期バイアス誤差
設定器の出力を他方の入力として、 それらセンサ姿勢及び位置のバイアス誤差を状態変数と
した線形状態方程式の観測行列を算出する観測行列生成
手段と、 上記観測行列からセンサ姿勢及び位置のバイアス誤差推
定値の共分散行列を算出するセンサ姿勢及び位置のバイ
アス誤差推定値評価手段と、 上記観測行列と上記センサ姿勢及び位置のバイアス誤差
推定値の共分散行列とからセンサ姿勢及び位置のバイア
ス誤差推定値の仮の値を算出するセンサ姿勢及び位置の
バイアス誤差推定値算出手段と、 上記センサ姿勢及び位置のバイアス誤差の初期値もしく
は前回のバイアス誤差推定値からセンサ姿勢及び位置の
バイアス誤差推定値の仮の値を差し引いて累積演算結果
からセンサ姿勢及び位置のバイアス誤差推定値を出力す
る手段と、を備えたことを特徴とするセンサ姿勢及び位
置のバイアス誤差推定装置。 - 【請求項４】 互いに離れた位置にある２つのセンサ位
置の地球の半径を設定する曲率設定器と、 上記２つのセンサから観測器を介してｋ個の目標の位置
を観測し各センサを原点とする直交座標のｘ座標及びｙ
座標と、海面からの高さを用いた基平面座標に変換した
出力と、上記２つのセンサの位置を基準の基平面座標に
て計測したセンサ位置計測器の出力とを加算して、上記
基準の基平面座標にて上記目標の観測位置を求めて一方
の入力とし、 上記センサ自体に起因しその観測諸元である目標の距
離，仰角，方位角各々に含まれるバイアス誤差をセンサ
姿勢のバイアス誤差と定義し、上記センサ位置の計測値
に含まれるバイアス誤差をセンサ位置のバイアス誤差と
定義し、それらセンサの姿勢及び位置のバイアス誤差の
初期値を設定するセンサ姿勢及び位置の初期バイアス誤
差設定器の出力を他方の入力として、 それらセンサの姿勢及び位置のバイアス誤差を状態変数
とした線形状態方程式の観測行列を算出する観測行列生
成手段と、 上記観測行列からセンサ姿勢及び位置のバイアス誤差推
定値の共分散行列を算出するセンサ姿勢及び位置のバイ
アス誤差推定値評価手段と、 上記観測行列と上記センサ姿勢及び位置のバイアス誤差
推定値の共分散行列とからセンサ姿勢及び位置のバイア
ス誤差推定値の仮の値を算出するセンサ姿勢及び位置の
バイアス誤差推定手段と、 上記センサ姿勢及び位置のバイアス誤差の初期値もしく
は前回のバイアス誤差推定値からセンサ姿勢及び位置の
バイアス誤差推定値の仮の値を差し引いて累積演算結果
からセンサ姿勢及び位置のバイアス誤差推定値を出力す
る手段と、を備えたことを特徴とするセンサ姿勢及び位
置のバイアス誤差推定装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP12954997A JP3439068B2 (ja) | 1997-05-20 | 1997-05-20 | センサ姿勢及び位置のバイアス誤差推定装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP12954997A JP3439068B2 (ja) | 1997-05-20 | 1997-05-20 | センサ姿勢及び位置のバイアス誤差推定装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH10319108A true JPH10319108A (ja) | 1998-12-04 |
| JP3439068B2 JP3439068B2 (ja) | 2003-08-25 |
Family
ID=15012265
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP12954997A Expired - Fee Related JP3439068B2 (ja) | 1997-05-20 | 1997-05-20 | センサ姿勢及び位置のバイアス誤差推定装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3439068B2 (ja) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006007940A (ja) * | 2004-06-24 | 2006-01-12 | Fujitsu Ltd | レーダ装置の校正方法、レーダ装置、監視システム、プログラム |
| JP2008525799A (ja) * | 2004-12-22 | 2008-07-17 | レイセオン・カンパニー | レーダアレイを較正するシステム及び技法 |
| JP2008175683A (ja) * | 2007-01-18 | 2008-07-31 | Mitsubishi Electric Corp | センサのバイアス誤差推定装置 |
| JP2012088135A (ja) * | 2010-10-19 | 2012-05-10 | Advanced Telecommunication Research Institute International | 距離センサのキャリブレーション装置、キャリブレーションプログラムおよびキャリブレーション方法 |
| JP2018146351A (ja) * | 2017-03-03 | 2018-09-20 | 株式会社東芝 | マルチセンサシステム、センサバイアス推定装置、センサバイアス推定方法及びセンサバイアス推定プログラム |
-
1997
- 1997-05-20 JP JP12954997A patent/JP3439068B2/ja not_active Expired - Fee Related
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2006007940A (ja) * | 2004-06-24 | 2006-01-12 | Fujitsu Ltd | レーダ装置の校正方法、レーダ装置、監視システム、プログラム |
| JP2008525799A (ja) * | 2004-12-22 | 2008-07-17 | レイセオン・カンパニー | レーダアレイを較正するシステム及び技法 |
| JP2008175683A (ja) * | 2007-01-18 | 2008-07-31 | Mitsubishi Electric Corp | センサのバイアス誤差推定装置 |
| JP2012088135A (ja) * | 2010-10-19 | 2012-05-10 | Advanced Telecommunication Research Institute International | 距離センサのキャリブレーション装置、キャリブレーションプログラムおよびキャリブレーション方法 |
| JP2018146351A (ja) * | 2017-03-03 | 2018-09-20 | 株式会社東芝 | マルチセンサシステム、センサバイアス推定装置、センサバイアス推定方法及びセンサバイアス推定プログラム |
Also Published As
| Publication number | Publication date |
|---|---|
| JP3439068B2 (ja) | 2003-08-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111207774A (zh) | 一种用于激光-imu外参标定的方法及系统 | |
| JP4876204B2 (ja) | 小型姿勢センサ | |
| EP2169422B1 (en) | System and method for acoustic tracking an underwater vehicle trajectory | |
| JP6191145B2 (ja) | オフセット推定装置及びプログラム | |
| Kelly et al. | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration | |
| CN112815939B (zh) | 移动机器人的位姿估计方法及计算机可读存储介质 | |
| US20060025688A1 (en) | Blood flow visualizing diagnostic apparatus | |
| JP2011027623A (ja) | 位置姿勢計測方法及び装置 | |
| CN111707261A (zh) | 一种微型无人机高速感知和定位方法 | |
| Tsao et al. | Observability analysis and performance evaluation of EKF-based visual-inertial odometry with online intrinsic camera parameter calibration | |
| JP6548305B2 (ja) | 進行方向推定装置及び進行方向推定方法 | |
| JP2013096724A (ja) | 状態推定装置 | |
| JPH10319108A (ja) | センサ姿勢及び位置のバイアス誤差推定装置 | |
| JP2013122384A (ja) | カルマンフィルタ、及び、状態推定装置 | |
| JP2013185898A (ja) | 状態推定装置 | |
| JP4804371B2 (ja) | センサのバイアス誤差推定装置 | |
| KR101140379B1 (ko) | 리 대수 및 칼만 필터를 이용한 자세 추정 방법 및 장치 | |
| JP2013088162A (ja) | 状態推定装置 | |
| JP4967724B2 (ja) | 現在地測位装置及び方法 | |
| CN106931879B (zh) | 一种双目误差测量方法、装置及系统 | |
| JP2001091622A (ja) | 目標位置観測装置 | |
| JP3302870B2 (ja) | センサのバイアス誤差推定装置 | |
| JP3218181B2 (ja) | センサのバイアス誤差推定装置 | |
| JPH08271247A (ja) | センサのアライメント誤差推定装置 | |
| JP2018063142A (ja) | 運動パラメータ推定装置、運動パラメータ推定方法及びプログラム |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080613 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20080613 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20090613 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100613 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100613 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110613 Year of fee payment: 8 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120613 Year of fee payment: 9 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130613 Year of fee payment: 10 |
|
| LAPS | Cancellation because of no payment of annual fees |