JP2008082931A

JP2008082931A - 慣性航法を使用した実時間位置測量のための方法及び装置

Info

Publication number: JP2008082931A
Application number: JP2006264471A
Authority: JP
Inventors: Kenneth S Morgan; ケニース・エス・モルガン; Jeffrey A Vanguider; ジェフリー・エイ・ファンギダー; John S White; ジョン・エス・ホワイト
Original assignee: Honeywell International Inc
Current assignee: Honeywell International Inc
Priority date: 2006-09-28
Filing date: 2006-09-28
Publication date: 2008-04-10
Anticipated expiration: 2026-09-28
Also published as: JP5046601B2

Abstract

【課題】慣性航法システムを使用して２つの地点の相対位置を決定する方法を提供する。
【解決手段】本方法は、ＩＮＳ（１０）が第１の位置なあるときの位置誤差状態及び位置解を推定するステップと、ＩＮＳが第２の位置にあるときの位置誤差状態及び位置解を推定するステップと、ＩＮＳを第１の位置に戻すステップとを含む。第１及び第２の位置誤差状態の推定値は、ＩＮＳを第２の位置から第１の位置へ戻す移行の間に生成された相関関係に基づいて調整される。
【選択図】図１

Description

本発明は一般に、車両の位置決定に関し、より詳細には慣性航法システム（ＩＮＳ）を使用した実時間位置測量のための方法及び装置に関する。

車両の報告された位置の精度は、多くの車両の用途に関して重要なものである。例えば、それぞれの砲との間の射程、方位、仰角に関するデータが非常に正確でなければならない砲配置や照準の用途では、正確な位置情報が望まれる。全地球測位システム（ＧＰＳ（グローバル・ポジショニング・システム））は、かなり正確な位置情報のための１つの既知の情報源である。しかし、このような正確な位置情報を使用する全てのシステムが、ＧＰＳ機能を備えている訳ではない。

一態様では、慣性航法システムを使用して、２以上の地点の相対位置を決定する方法が提供される。この方法は、第１の位置でＩＮＳを使用して位置誤差状態及び位置解（position solution）を推定するステップと、１又は複数の後続の位置でＩＮＳを使用して位置誤差状態及び位置解を推定するステップと、ＩＮＳを第１の位置に戻すステップと、ＩＮＳを第２の位置から第１の位置へ戻す移行の間に生成された相関関係に基づいて、第１及び後続の位置誤差状態の推定値を調整するステップとを含む。

他の態様では、２以上の地点の相対位置を決定するシステムが提供される。このシステムは、慣性航法システムと、ＩＮＳと通信するように結合されたコンピュータ・ベースのシステムとを含む。ＩＮＳは、少なくとも２つのＩＮＳ位置についての位置誤差状態及び位置解を推定するように構成される。このコンピュータ・ベースのシステムは更に、ＩＮＳが第１のＩＮＳ位置である地点Ａへ、１又は複数の後続のＩＮＳ位置から戻る移行の間に生成された相関関係に基づいて、位置誤差状態の推定値を調整するようにプログラムされたフィルタも含む。

他の態様では、慣性航法システムから慣性データを受信するように構成されたデータ管理システムが提供される。このデータ管理システムは、ＩＮＳ位置に対する位置誤差状態及び位置解を推定するようにプログラムされた少なくとも１つのプロセッサと、後続のＩＮＳ位置から第１のＩＮＳ位置へ戻るＩＮＳの移行の間に生成された相関関係に基づいて位置誤差状態の推定値を調整するようにプログラムされたフィルタとを含む。

本明細書に記載の方法及び装置は、慣性航法をベースとして使用して、地球上の２つの場所の相対位置を非常に正確に測定することを可能にする。更に、この位置は、全地球測位システム（ＧＰＳ）などの外部補助ソースを使用することなしに、正確に決定される。

図１は、例えばデータ管理システムなどのコンピュータ・ベース・システム２０にインタフェースされる慣性航法システム（ＩＮＳ）１０のブロック図である。少なくとも１つの実施形態では、慣性データは、１又は複数のジャイロスコープ３０及び加速度計４０からＩＮＳ１０へ提供される。ＩＮＳ１０は、ジャイロスコープ３０及び加速度計４０から受信した慣性データをフォーマットし、フォーマットされた慣性データをコンピュータ・ベース・システム２０へ送信する。

コンピュータ・ベース・システム２０は、内部のコンピュータに、慣性航法システム（ＩＮＳ）１０から受信したデータを処理させるためのコンピュータ・プログラムを含む。一実施形態では、コンピュータ・プログラムの機能強化により、２地点の相対位置を、スタンドアローンのＩＮＳ１０が測定するよりも、更に正確に決定することができるようになる。より具体的には、コンピュータ・プログラムの内部にデータ平滑技術を組み込むことにより、冗長なオフライン・データ処理を必要とせずに、位置についての結果を提供する。

或る特定の実施形態では、コンピュータ・ベース・システム２０内で動作するプログラムは、例えばカルマン・フィルタなどのような再帰型（巡回型）フィルタ５０を含む。再帰型フィルタは、ＩＮＳ１０から受信した慣性データに関連する誤差を推定し訂正するように構成される。再帰型フィルタ５０は、ＩＮＳ速度誤差伝播の時々の観測値として零点位置変化の更新を使用する。この実施形態では、フィルタ５０は、２つの推定地点での位置誤差を表す６つの追加の状態を含むように拡張され、更にこれらの状態を使用するための処理が組み込まれる。

この２つの推定地点は、本明細書では、地点Ａ及び地点Ｂ、又は第１の位置及び第２の位置と呼ぶこともあるが、６つの追加の状態を使用するために必要な特別な処理を含む。地点Ａ及び地点Ｂならびにそれらの間のサンプル測量パターンが図２に示されている。２つの推定地点での位置誤差を表す６つの追加の状態を得るために、ＩＮＳ１０は地点Ａへ移送され、その地点で、或る時間の間、例えば約３０秒間、実質的に静止した状態で維持される。コンピュータ・ベース・システム２０は、再帰型フィルタ５０を介して、現在位置誤差状態を地点Ａ誤差状態へコピーするようにプログラムされており、地点Ａ誤差状態は６つの追加の状態の一部である。コンピュータ・ベース・システム２０は更に、地点Ａの現在の位置解も格納する。

ＩＮＳ１０は地点Ｂへ移送され、ほぼ同じ時間の間、静止状態に維持される。フィルタ５０は、現在位置誤差状態を地点Ｂ誤差状態へコピーするように再び更新され、地点Ｂの現在の位置解が格納される。ＩＮＳ１０は再び地点Ａに戻され、ある時間の間、静止状態に維持される。この点で、フィルタ５０は、現在の位置解と以前に格納された地点Ａ位置との間の差分を用いて更新される。

フィルタ５０のこの更新のためのフィルタ観測行列は、現在位置誤差状態から地点Ａ誤差状態を引いたものを観測するように形成される。フィルタ５０のこの更新は、地点Ａと地点Ｂとの間の移行の間にフィルタ共分散行列の形で生成された相関関係に基づいて、地点Ａ及び地点Ｂの位置誤差の推定値を調整する。最後の点Ａ及びＢの位置推定値は、地点Ａ及びＢの推定された誤差を以前に格納された位置の値に加算することによって、形成される。

以下の各式は、上述のカルマン・フィルタの実装の例を示すものである。この場合、ＩＮＳ１０は、ワンダ方位角フレーム（wander azimuth frame）で動作し、測量はユニバーサル横メルカトール・グリッド・フレーム（universal transverse mercator grid frame）で行われる。

コンピュータ・ベース・システム２０内のフィルタ５０のカルマン・フィルタの実施形態は、ＩＮＳ１０からの慣性測定データを処理して、位置および速度に関する実時間の推定値を生成する。より具体的には、フィルタ５０は、それぞれの状態推定値の予測誤差と、例えば地点Ａ及びＢに関連する様々な誤差状態の間の相関関係とに関する情報を含む誤差共分散行列を維持する。この誤差共分散行列は、慣性データが下記の標準式を用いて処理されると、適時に伝播される。

Ｐ_Ｎ＋１＝ΦＰ_ＮΦ^Ｔ＋Ｑ。この式で、Ｐは誤差共分散行列、Φは状態遷移行列、Ｑはプラント雑音行列である。

車両が地点Ａに存在するとき、Φ行列は、現在の誤差状態を追加の地点Ａ誤差状態へコピーするために、１フィルタ・サイクルごとに調整される。この変更は、１つのフィルタ・サイクルについて以下のエントリを追加する形を取る。具体的には下記のようである。

Φ（ＰＯＳ＿Ａ＿Ｅ，ＰＸ）＝−ｓｉｎ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ｅ，ＰＹ）＝−ｃｏｓ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ｎ，ＰＸ）＝−ｃｏｓ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ｎ，ＰＹ）＝ｓｉｎ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ａ，ＰＺ）＝１．０

上記の式で、ＰＯＳ＿Ａ＿Ｅは地点Ａ東方向（easting）誤差状態、ＰＯＳ＿Ａ＿Ｎは地点Ａ北方向（northing）誤差状態、ＰＯＳ＿Ａ＿Ａは地点Ａ高度誤差状態、ＰＸはナビゲータ・ワンダ・フレームＸ誤差状態、ＰＹはナビゲータ・ワンダ・フレームＹ誤差状態、ＰＺはナビゲータ・ワンダ・フレームＺ誤差状態、Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄはワンダ・フレームとグリッド・フレームとの間の現在の方位角である。処理地点Ｂの現在の誤差状態は、ＰＯＳ＿ＡをＰＯＳ＿Ｂに置き換えたものに非常に類似している。

地点Ａに戻ると、コンピュータ・ベース・システム２０は、フィルタ５０へ測定値更新を適用するように構成される。測定値は、現在のナビゲーション位置と、地点Ａへの最初の訪問時に記録された位置との差分を含む。地点Ａ位置の更新のための以下の観察行列エントリの結果となる差分は下記に示される。具体的には下記のようである。

Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅ，ＰＸ）＝ｓｉｎ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅ，ＰＹ）＝ｃｏｓ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎ，ＰＸ）＝ｃｏｓ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎ，ＰＹ）＝−ｓｉｎ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ａ，ＰＺ）＝−１．０、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅ，ＰＯＳ＿Ａ＿Ｅ）＝１．０、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎ，ＰＯＳ＿Ａ＿Ｎ）＝１．０、及び
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ａ，ＰＯＳ＿Ａ＿Ａ）＝１．０

上記の式で、ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅは東方向位置の差分の観測値、ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎは北方向位置の差分の観測値、ＯＢＳ＿ＰＯＳ＿Ａ＿Ａは垂直位置の差分の観測値である。

位置更新のためのカルマン・ゲインは、標準式である、Ｇ＝ＰＨ^Ｔ（ＨＰＨ^Ｔ＋Ｒ）^−１を使用して計算される。この式で、Ｒは測定値更新のための誤差共分散行列であり、車両を地点Ａに正確に戻す際の誤差を表し、従って、それぞれの測定軸について小さい数となる。従って、測定誤差は無相関だと見なすことができる。

地点Ｂ位置の誤差状態は明示的には観測されないが、それらは地点Ａに戻る間にＰ行列の伝播（上式を使用する）によって生成された相互相関関係によって更新される。地点Ａ及びＢの位置についての最終の解は、測量車両によって最初に訪問されたときに記録された位置から、ＰＯＳ＿Ａ＿・・・及びＰＯＳ＿Ｂ＿・・・の状態に含まれる処理によって提供される誤差推定値を減算したものである。より具体的には下記のようである。

上記の式で、ＰＡ＿Ｅ、ＰＡ＿Ｎ、ＰＡ＿Ａは、地点Ａへの最初の訪問で記録された東方向、北方向、及び高度のナビゲータ位置座標値であり、ＰＢ＿Ｅ、ＰＢ＿Ｎ、ＰＢ＿Ａは、地点Ｂへの訪問時に記録された東方向、北方向、及び高度のナビゲータ位置座標値であり、

は、フィルタ状態ベクトルである。

図３は、慣性航法システム１０を使用して２つの地点の相対位置を決定するための上述の方法を一般的に示す流れ図１００である。この方法は、ＩＮＳが第１の位置にあるときの位置誤差状態及び位置解を推定するステップ１０２と、ＩＮＳが第２の位置にあるときの位置誤差状態及び位置解を推定するステップ１０４と、ＩＮＳを第１の位置に戻すステップ１０６とを含む。本明細書で地点Ａと呼ばれることもある第１の位置に戻ると、第１及び第２の位置誤差状態の推定値は、ＩＮＳを第２の位置から第１の位置へ戻す移行の間に生成された相関関係に基づいて調整される（１０８）。

上述の方法及びシステムは、ＩＮＳが地点Ａから地点Ｂに移動し、再度地点Ａに戻るものに関して記載されているが、ＩＮＳが地点Ａに戻る前に、後続の位置と呼ばれることもある複数の中間データ地点へ移動させることを用いるか又はそれを含む代替の実施形態を実装することもできる。他の代替実施形態では、地点Ａを測量された地点とすることができ、また、位置が既知である後続の位置の１つを測量された地点とすることもできる。測量された地点Ａを含む実施形態では、ＩＮＳは、その測量された地点Ａから複数の中間地点に移動し、更に他の測量された地点へ移動するか、又は測量された地点Ａへ戻る。

これらの代替実施形態は、中間（非測量）地点の位置の良好な推定値を提供する。当業者には理解されるが、ＩＮＳ位置が決定されるそれぞれの後続の位置（例えば、それぞれの中間地点）では、それぞれの中間地点の誤差状態を用いて再帰型フィルタを構成することが含まれる。上述の２位置を用いる方法（例えば、地点Ａ及び地点Ｂ）と同様に、ＩＮＳ（例えば、ＩＮＳ１０）がそれぞれの後続の位置へ移送されて、或る時間の間、静止状態が維持されると、再帰型フィルタが更新されて現在の位置誤差状態が個々の位置誤差状態にコピーされ、それぞれの後続の位置についての現在の位置解が格納される。

ＩＮＳが地点Ａに戻され、或る時間の間、静止状態に維持されると、再帰型フィルタは、現在の位置解と以前に格納された地点Ａ位置との間の差分を用いて更新される。ＩＮＳが開始点Ａ以外の測量地点に移動されるときにも、同様の計算が実施される。何れの実施形態でも、再帰型フィルタ５０を更新するためのフィルタ観測行列は、様々な中間地点間の移行の間にフィルタ共分散行列に生成された相関関係に基づいての中間地点位置誤差の推定値を調整するために、現在の位置誤差状態から開始地点Ａ誤差状態を減算したものを観測するように形成される。最終の位置推定値は、中間地点に対する推定誤差を、以前に格納された位置の値に加算することによって、形成される。例えばカルマン・フィルタなどのような再帰型フィルタは、誤差共分散行列を維持し、この誤差共分散行列は、それぞれの状態推定値の予想誤差と、例えば、地点Ａ及び地点Ｂならびにそれらの間の任意の中間地点に関連する様々な誤差状態の間の相関関係とに関する情報を含む。この誤差共分散行列は、慣性データが前述と同様に処理されると適時に伝播される。

本発明を様々な個別の実施形態に関して説明してきたが、特許請求の範囲に記載の趣旨及び範囲を逸脱することなく本発明を変更して実施できることが、当業者には理解されよう。

図１は、コンピュータ・ベースのシステムと通信するように結合された慣性航法システムのブロック図である。図２は、位置測量に使用されるサンプル・パターンである。図３は、位置測量方法の流れ図である。

Claims

２以上の地点の相対位置を決定するシステムであって、
慣性航法システム（ＩＮＳ）（１０）と、
前記ＩＮＳと通信するように結合され、少なくとも２つのＩＮＳ位置に対する位置誤差状態及び位置解を推定するように構成されたコンピュータ・ベース・システム（２０）であって、前記ＩＮＳが、第１のＩＮＳ位置である地点Ａへ、１又は複数の後続のＩＮＳ位置から戻る移行の間に生成された相関関係に基づいて、前記位置誤差状態の推定値を調整するようにプログラムされたフィルタ（５０）を含むコンピュータ・ベース・システムと
を備えるシステム。
請求項１に記載のシステムであって、前記フィルタ（５０）が、少なくとも２つの前記ＩＮＳ（１０）位置での位置誤差を表すための複数の追加の誤差状態を含むように拡張されたカルマン・フィルタを含む、システム。
請求項２に記載のシステムであって、前記フィルタ（５０）が、それぞれのＩＮＳ（１０）位置に関連するそれぞれの状態推定値における予想誤差と様々な誤差状態の間の相関関係とに関する情報を含む誤差共分散行列を含む、システム。
請求項３に記載のシステムであって、前記誤差共分散行列が、Ｐ_Ｎ＋１＝ΦＰ_ＮΦ^Ｔ＋Ｑにより前記ＩＮＳ（１０）からの慣性データとして適時に伝播され、上記の式において、Ｐは誤差共分散行列、Φは状態遷移行列、Ｑはプラント雑音行列である、システム。
請求項４に記載のシステムであって、
前記ＩＮＳ（１０）の第１の位置である地点Ａに対して、前記状態遷移行列Φが、下記の式、
Φ（ＰＯＳ＿Ａ＿Ｅ，ＰＸ）＝−ｓｉｎ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ｅ，ＰＹ）＝−ｃｏｓ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ｎ，ＰＸ）＝−ｃｏｓ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ｎ，ＰＹ）＝ｓｉｎ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ａ＿Ａ，ＰＺ）＝１．０
により前記現在の誤差状態を少なくとも１つの前記追加の誤差状態にコピーするように、前記フィルタ（５０）の１サイクルに対して調整され、前記ＩＮＳの第２の位置である地点Ｂに対して、前記状態遷移行列Φが、下記の式、
Φ（ＰＯＳ＿Ｂ＿Ｅ，ＰＸ）＝−ｓｉｎ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ｂ＿Ｅ，ＰＹ）＝−ｃｏｓ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ｂ＿Ｎ，ＰＸ）＝−ｃｏｓ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ｂ＿Ｎ，ＰＹ）＝ｓｉｎ（Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄ）
Φ（ＰＯＳ＿Ｂ＿Ａ，ＰＺ）＝１．０
により前記現在の誤差状態を少なくとも１つの前記追加の誤差状態にコピーするように、前記フィルタの１サイクルに対して調整され、上記の式において、ＰＯＳ＿Ａ＿Ｅは地点Ａ東方向誤差状態、ＰＯＳ＿Ａ＿Ｎは地点Ａ北方向誤差状態、ＰＯＳ＿Ａ＿Ａは地点Ａ高度誤差状態、ＰＯＳ＿Ｂ＿Ｅは地点Ｂ東方向誤差状態、ＰＯＳ＿Ｂ＿Ｎは地点Ｂ北方向誤差状態、ＰＯＳ＿Ｂ＿Ａは地点Ｂ高度誤差状態、ＰＸはナビゲータ・ワンダ・フレームＸ誤差状態、ＰＹはナビゲータ・ワンダ・フレームＹ誤差状態、ＰＺはナビゲータ・ワンダ・フレームＺ誤差状態、Ｗａｎｄｅｒ＿ｔｏ＿Ｇｒｉｄはワンダ・フレームとグリッド・フレームとの間の現在の方位角である、
システム。
請求項５に記載のシステムであって、前記ＩＮＳ（１０）の地点Ａへ戻ると、前記コンピュータ・ベース・システム（２０）は前記フィルタに対して測定更新を適用し、前記測定更新は、現在のナビゲーション位置と、前記ＩＮＳが前記地点Ａに最初に訪れたときに記録された位置との間の差分を含む、システム。
請求項６に記載のシステムであって、前記差分が下記の式、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅ，ＰＸ）＝ｓｉｎ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅ，ＰＹ）＝ｃｏｓ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎ，ＰＸ）＝ｃｏｓ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎ，ＰＹ）＝−ｓｉｎ（Ｗａｎｄｅｒ＿Ｔｏ＿Ｇｒｉｄ）、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ａ，ＰＺ）＝−１．０、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅ，ＰＯＳ＿Ａ＿Ｅ）＝１．０、
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎ，ＰＯＳ＿Ａ＿Ｎ）＝１．０、及び
Ｈ（ＯＢＳ＿ＰＯＳ＿Ａ＿Ａ，ＰＯＳ＿Ａ＿Ａ）＝１．０
により地点Ａ位置の前記更新のための観測行列エントリを生じ、上記の式において、ＯＢＳ＿ＰＯＳ＿Ａ＿Ｅは東方向位置における前記差分の観測値、ＯＢＳ＿ＰＯＳ＿Ａ＿Ｎは北方向位置における前記差分の観測値、ＯＢＳ＿ＰＯＳ＿Ａ＿Ａは垂直位置における前記差分の観測値である、システム。
請求項１に記載のシステムであって、前記フィルタ（５０）が、Ｇ＝ＰＨ^Ｔ（ＨＰＨ^Ｔ＋Ｒ）^−１に従って位置更新に対するゲインを計算するようにプログラムされ、上記の式において、Ｒは測定値更新についての前記誤差共分散行列であり、前記車両を地点Ａに正確に戻す際の誤差を表す、システム。
請求項１に記載のシステムであって、前記コンピュータ・ベース・システムが下記の式、

に従って地点Ａ及び後続の位置である地点Ｂの位置を決定するようにプログラムされ、上記の式において、ＰＡ＿Ｅ、ＰＡ＿Ｎ、ＰＡ＿Ａは地点Ａへ最初に訪れたときに記録された東方向、北方向、及び高度のナビゲータ位置座標であり、ＰＢ＿Ｅ、ＰＢ＿Ｎ、ＰＢ＿Ａは地点Ｂへ訪れた時に記録された東方向、北方向、及び高度のナビゲータ位置座標であり、ＰＯＳ＿Ａ＿Ｅは地点Ａ東方向誤差状態、ＰＯＳ＿Ａ＿Ｎは地点Ａ北方向誤差状態、ＰＯＳ＿Ａ＿Ａは地点Ａ高度誤差状態であり、ＰＯＳ＿Ｂ＿Ｅは地点Ｂ東方向誤差状態、ＰＯＳ＿Ｂ＿Ｎは地点Ｂ北方向誤差状態、ＰＯＳ＿Ｂ＿Ａは地点Ｂ高度誤差状態であり、

はフィルタ（５０）状態ベクトルである、
システム。