JP4527171B2

JP4527171B2 - 単一ｇｐｓと慣性データ（加速度，角速度）によるビークル姿勢角の計測方法

Info

Publication number: JP4527171B2
Application number: JP2008544597A
Authority: JP
Inventors: 健二高畑; 浩一岡村; 智明樋口; 龍田中; 忠司伊藤; 盛雄高浜
Original assignee: Nagoya University NUC; Tokai National Higher Education and Research System NUC
Current assignee: Nagoya University NUC; Tokai National Higher Education and Research System NUC
Priority date: 2007-05-18
Filing date: 2007-05-18
Publication date: 2010-08-18
Anticipated expiration: 2027-05-18
Also published as: JPWO2008142757A1; WO2008142757A1

Description

本発明は、ビークルの姿勢角計測方法に関する。

自動車を始めとするビークルの走行に関する姿勢角計測方法には、移動体上に複数の衛星信号受信アンテナを設置して測定する、特開２００１−１９４４４２号公報がある。
この公報によれば、移動体上に２つ以上の衛星信号受信アンテナを設置し、複数の衛星からの衛星信号搬送波（キャリア）位相の位相差を利用して、基準とするアンテナ（基準局）から見たその他のアンテナの相対位置をセンチメートルレベルの高精度で求め、その相対位置から移動体の姿勢角を計測する。例えば、２つのアンテナで構成した場合は、移動体の前後方向に２アンテナを設置すれば、方位角とピッチ角が計測できる。また、その２つのアンテナと直交した位置に３台目のアンテナを設置すれば、ロール角も計測することができる。

特開２００１−１９４４４２号公報

前記公報の方法によると、複数の衛星信号受信機を使用しての姿勢角計測は原理的に容易であが、固定局（基準局）と移動体上に最低でも３つの衛星信号受信機が必要であり、コストおよび、ビークルへの搭載占有容積がかさむことになり実用上の障害となる。
そこで、本発明では、高精度な測位ができるＧＰＳ受信機を１つと、安価（低精度）な慣性センサ（ジャイロおよび加速度センサ）を搭載し、慣性センサのみでは発生を避けられない姿勢角誤差に起因する加速度誤差を、ＧＰＳで求めた位置から推定する高精度な加速度で補正して高精度な姿勢角計測方法を提供するものである。

本願の発明は、ビークル（移動体）にＧＰＳとジャイロと加速度センサを搭載してビークルの姿勢角測定方法である。
そして、時刻ｔ_ｉにおいて、ＧＰＳによってビークルの位置（Ｒ_X，Ｒ_Y，Ｒ_Z）が測定され、ジャイロによってｘ軸回りのロール角速度ｐ^〜、ｙ軸回りのピッチ角速度ｑ^〜、ｚ軸回りのヨー角速度ｒ^〜が測定される。又、加速度センサによってｘ軸方向の縦加速度ａ^〜 _x、ｙ軸方向の横加速度ａ^〜 _y、ｚ軸方向の垂直加速度ａ^〜 _zが測定される。

次に、前記求めた時刻ｔ_ｉにおける各測定値を用いて下記の演算を行う。
（１）前記ビークルの位置（Ｒ_X，Ｒ_Y，Ｒ_Z）から地球座標系の推定加速度（Ａ＾_X，Ａ＾_Y，Ａ^＾ _Z）求める。この推定加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）は前記ビークルの位置（Ｒ_X，Ｒ_Y，Ｒ_Z）を観測データとするカルマンフィルタで推定する。このカルマンフィルタは後述の式（１）の運動方程式を基準に構成する。
（２）加速度センサによるビークルの観測加速度（ａ^〜 _x，ａ^〜 _y，ａ^〜 _z）を求める。
（３）前記ジャイロで測定の角速度（ｐ^〜，ｑ^〜，ｒ^〜）から姿勢角誤差を含んだロール角φ^〜、ピッチ角θ^〜及びヨー角ψ^〜を求め、それらの値から姿勢角誤差を含んだ座標変換行列Ｅ^-1を求める。

（４）ビークル座標系の観測加速度（ａ^〜 _x，ａ^〜 _y，ａ^〜 _z）に姿勢角誤差を含んだ前記で求めた座標変換行列Ｅ^-1をかけ算することによって、地球座標系加速度（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _z）が求まる。
（５）前記ＧＰＳから求めた地球座標系の推定加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）と前記加速度センサ観測値から求めた地球座標系の観測加速度（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _z）の差を推定加速度誤差（δＡ＾_X，δＡ＾_Y，δＡ＾_Z）とする。
（６）そして、ＧＰＳから求めた推定加速度Ａ_YZ（ベクトル要素：Ａ＾_Y，Ａ＾_Z）と推定加速度誤差δＡ_YZ（ベクトル要素：δＡ＾_Y，δＡ＾_Z）の関係から姿勢角誤差δφ＝ｔａｎ^-1｜δＡ_YZ｜／｜Ａ_YZ｜、また、ＧＰＳから求めた推定加速度Ａ_XZ（ベクトル要素：Ａ＾_X，Ａ＾_Z）と推定加速度誤差δＡ_XZ（ベクトル要素：δＡ＾_X，δＡ＾_Z）の関係から姿勢角誤差δθ＝ｔａｎ^-1｜δＡ_XZ｜／｜Ａ_XZ｜、さらに、ＧＰＳから求めた推定加速度Ａ_XY（ベクトル要素：Ａ＾_x，Ａ＾_Y）と推定加速度誤差δＡ_XY（ベクトル要素：δＡ＾_X，δＡ＾_Y）の関係から方位角誤差δψ＝ｔａｎ^-1｜δＡ_XY｜／｜Ａ_XY｜をそれぞれ求める。又、前記推定加速度Ａ_YZと推定加速度誤差δＡ_YZの外積によりδφ、推定加速度Ａ_XZと推定加速度誤差δＡ_XZの外積によりδθ、推定加速度Ａ_XYと推定加速度誤差δＡ_XYの外積によりδψの極性判定を行う。

あるいは、前記（６）においては姿勢角誤差を地球座標系で求めたが、ビークル座標系で求めてもよい。この場合、ビークル座標系の推定加速度ａ_yz（ベクトル要素：a^_y，a^_z）と加速度誤差δa_yz（ベクトル要素：δa^_y，δa^_z）から姿勢角誤差δφ＝ｔａｎ^-1｜δａ_yz｜／｜ａ_yz｜，推定加速度a_xz（ベクトル要素：a^_x，a^_z）と加速度誤差δa_xz（ベクトル要素：δa^_X，δa^_z）の関係から姿勢角誤差δθ＝ｔａｎ^-1｜δａ_xz｜／｜ａ_xz｜，推定加速度a_xy（ベクトル要素：a^_x，a^_y）と加速度誤差δa_xy（ベクトル要素：δa^_X，δa^_y）関係から姿勢角誤差δψ＝ｔａｎ^-1｜δａ_xy｜／｜ａ_xy｜をそれぞれ求める。また、前記推定加速度a_YZと推定加速度誤差δa_YZの外積によりδφ、推定加速度a_XZと推定加速度誤差δa_XZの外積によりδθ、推定加速度a_XYと推定加速度誤差δa_XYの外積によりδψの極性判定を行う。

（７）そして、前記極性を考慮した姿勢角誤差（δφ、δθ、δψ）を用いて、下記の算出式で推定姿勢角（ロール角φ、ピッチ角θ、ヨー角ψ）を求める。
推定ロール角φ＝（誤差を含んだロール角φ^〜）−（ロール姿勢角誤差δφ）
推定ピッチ角θ＝（誤差を含んだピッチ角θ^〜）−（ピッチ姿勢角誤差δθ）
推定ヨー角ψ ＝（誤差を含んだヨー角ψ^〜） −（方位角誤差δψ）
（８）以上から、時刻ｔ_ｉにおける推定ロール角φ、推定ピッチ角θ、推定ヨー角ψが求まる。
以上の過程（１）〜（７）を時刻ｔ_ｉ、ｔ_ｉ＋δｔ、…と順次求めることによって、ビークルの移動時における姿勢角が求まる。

本願の発明は、ビークルに各１台のＧＰＳとジャイロと加速度センサの搭載により、ビークルの回転系と並進系の運動方程式において姿勢角誤差を含んだ座標変換行列Ｅ^-1を用いて、加速度誤差を含む地球座標系加速度（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _Z）が求まる。一方，前記ビークルのＧＰＳ測位値（R_X，R_Y，R_Z）からほとんど誤差のない正確な地球座標系の推定加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）が求まる。そして、地球座標系加速度（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _Z）と推定加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）からカルマンフィルタで推定加速度誤差（δＡ＾_X、δＡ＾_Y，δＡ＾_Z）を求める。この推定加速度誤差（δＡ＾_X、δＡ＾_Y，δＡ＾_Z）と高精度な地球座標系の推定加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）の幾何学的関係から姿勢角誤差（δφ，δθ，δψ）を求めると共に、その姿勢角誤差（δφ，δθ，δψ）の極性を求める。そして，この姿勢角誤差を含む姿勢角から、極性を考慮した姿勢角誤差δφ，δθ，δψを差し引くことで，ロール角φ、ピッチ角θ及びヨー角ψの推定を可能にするので、簡便な設備で推定できる。

ＧＰＳとジャイロと加速度センサによる姿勢角推定方法の算出ブロック図である。（Ａ）はＧＰＳから求めた推定加速度Ａ_YZ（ベクトル要素：Ａ＾_Y，Ａ＾_Z）と推定加速度誤差δＡ_YZ（ベクトル要素：δＡ＾_Y，δＡ＾_Z）による姿勢角誤差δφ＝ｔａｎ^-1｜δＡ_YZ｜／｜Ａ_YZ｜の関係図である。（Ｂ）はＧＰＳから求めた推定加速度Ａ_XZ（ベクトル要素：Ａ＾_X,Ａ＾_Z）と推定加速度誤差δＡ_XZ（ベクトル要素：δＡ＾_X,δＡ＾_Z）による姿勢角誤差δθ＝ｔａｎ^-1｜δＡ_XZ｜／｜Ａ_XZ｜の関係図である。（Ｃ）はＧＰＳから求めた推定加速度Ａ_XY（ベクトル要素：Ａ＾_X,Ａ＾_Y）と推定加速度誤差δＡ_XY（ベクトル要素：δＡ＾_X,δＡ＾_Y）による方位角誤差δψ＝ｔａｎ^-1｜δＡ_XY｜／｜Ａ_XY｜の関係図である。

本発明の実施の一形態について説明する。ビークル（移動体）には１台のＧＰＳ（Global Positioning System）と、１台のジャイロ（３軸角速度センサ）と１台の３軸加速度センサが搭載してあると共に、下記する演算の実行をするソフトウエアを格納のＣＰＵ、メモリ等を備える演算装置を搭載している。尚、以下において、誤差を含む観測値（測定値）にはサフィックス「^〜」を付して表し、推定値にはサフィックス「^」を付して表す。
前記ＧＰＳによってビークルの位置（Ｒ_X,Ｒ_Y,Ｒ_Z）が測定され、前記ジャイロでは、x軸回りのロール角速度ｐ^〜、ｙ軸回りのピッチ角速度ｑ^〜、ｚ軸回りのヨー角速度ｒ^〜として測定される。又、加速度センサからビークルｘ軸方向の縦加速度ａ^〜 _x、ビークルｙ軸方向の横加速度ａ^〜 _y、ビークルｚ軸方向の垂直加速度ａ^〜 _zが測定データとして求まる。

次に、姿勢角であるビークルｘ軸回りのロール角φ、ビークルｙ軸回りのピッチ角θ、ビークルｚ軸回りのヨー角ψをカルマンフィルタで推定するために必要なビークルの運動方程式（状態方程式）を回転系の式（１）と並進系の式（２）を示す。

ここで、Ｗ^〜：姿勢誤差を含む姿勢角，ω：角速度（ｐ,ｑ,ｒ），εｄω：角速度ベクトルのゆらぎ，ｗｄω：駆動雑音、α：ビークル回転系の時定数の逆数，νω：観測雑音である。

ここで、Ｒ_G：位置、Ｖ：速度、Ａ：加速度、δＡ：姿勢角誤差による加速度誤差、ε_Ａ：加速度誤差のゆらぎ、ε_dA：姿勢誤差に基づく加速度のゆらぎ、W_A、W_dA：駆動雑音、γ：ビークル並進系の時定数の逆数，ν_R：GPSの観測雑音，ν_A：加速度計の観測雑音である。
加速度誤差δＡは白色雑音を整形フィルタを通して有色化したガウスマルコフ過程，加速度Ａのゆらぎは白色雑音で駆動される積分過程として加速度誤差δＡと加速度Ａの生成過程を明確に区別してある。

又、観測データｙは、ｙ1＝Ｒ_G＋ν_R、ｙ2＝Ａ＋δＡ＋ν_Aである。
このｙ1は、ＧＰＳで求める位置（Ｒ_X,Ｒ_Y,Ｒ_Z）＋観測雑音（ν_R）である。又、ｙ2はＡ＋δＡ＋ν_Aであり，加速度Ａと姿勢角誤差による加速度誤差δＡを線形結合した形で観測データを表現していることにより，加速度Ａと姿勢角誤差による加速度誤差δＡをカルマンフィルタが前記したように明確に区別した生成過程によって推定し，加速度Ａと加速度誤差δＡの分離を可能にしている。

次に、前記式（１）（２）を解くブロック図を図１に示し、この図１を参照して、姿勢角度のロール角φ、ピッチ角θ及びヨー角ψの推定について説明する。
尚、このブロック図に基づいて演算を実行するのは演算装置であり、算出する値は時刻ｔ_ｉにおける値であり、演算はδ時間毎に行う。

ＧＰＳで測定される位置（Ｒ_X，Ｒ_Y，Ｒ_Z）を微分すると推定速度Ｖ（Ｖ＾_X，Ｖ＾_Y，Ｖ＾_Z）が求まる。そして、更に、この推定速度Ｖを微分して推定加速度Ａ（Ａ＾_x，Ａ＾_Y，Ａ＾_ｚ）が求まる。この関係を状態方程式へ定式化して地球座標系カルマンフィルタを構築して加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）を推定する。
又、加速度センサからビークルｘ軸方向の縦加速度ａ^〜 _x、ビークルｙ軸方向の横加速度ａ^〜 _y、ビークルｚ軸方向の垂直加速度ａ^〜 _zを測定データとして、上記の状態方程式に加速度および加速度の揺らぎの状態量を併せた形で定式化してカルマンフィルタを構築する。
又、ジャイロで測定のビークルｘ軸回りのロール角速度ｐ^〜、ビークルｙ軸回りのピッチ角速度ｑ^〜、ビークルｚ軸回りのヨー角速度ｒ^〜を積分して、ロール角φ、ピッチ角θ及びヨー角ψを求める。この関係を状態方程式へ定式化して回転座標系カルマンフィルタを構築して(φ，θ，ψ)を推定する。

次に、加速度計が計測した観測加速度（ａ^〜 _x，ａ^〜 _y，ａ^〜 _z）を地球座標系の加速度に変換するために、座標変換行列Ｅ^-1（式（３））を用いる。

前記誤差を含むロール角φ^〜、ピッチ角θ^〜及びヨー角ψ^〜を座標変換行列Ｅ^-1（式（３））に代入して行列値を算出する。
そして、前記加速度計が計測した観測加速度（ａ^〜 _x，ａ^〜 _y，ａ^〜 _z）と座標変換行列Ｅ^-1をかけ算して、地球座標系加速度（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _Z）を求める。
次に、前記ＧＰＳ観測データから推定した正確な地球座標系の推定加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）と、前記地球座標系加速度（（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _Z）の差から、カルマンフィルタで推定加速度誤差（δＡ＾_X，δ＾_Y，δＡ＾_Z）を求める。

前記ＧＰＳ観測データから推定した正確な地球座標系の推定加速度（Ａ＾_X，Ａ＾_Y,Ａ＾_Z）と推定加速度誤差（δＡ＾_X，δＡ＾_Y，δＡ＾_Z）から、姿勢角誤差であるｘ軸回りのロール角δφ、ｙ軸回りのピッチ角δθ、ｚ軸回りのヨー角δψを求める。尚、前記ＧＰＳ観測データから推定した正確な地球座標系の推定加速度（Ａ＾_X，Ａ＾_Y，Ａ＾_Z）と推定加速度誤差（δＡ＾_X，δＡ＾_Y，δＡ＾_Z）は、座標変換の原理から直交するベクトルである。

誤差モデルと誤差の分離
カルマンフィルタで推定した加速度誤差δAは、地球座標におけるもので、ビークルに搭載して計測した加速度の座標変換精度に影響された姿勢角誤差（ロール角φ，ピッチ角θ，ヨー角ψ）に依存する。

(１)方位角誤差による加速度誤差
地球座標へ変換したビークルの運動を加速度計の計測値を基に、式（２）のＡ（Specific force）とδA(加速度誤差)加えた形で表すと、式（４）になる。
観測誤差および，重力成分を含まないビークルの運動加速度をSpecific forceと定義する。

尚、a_ψはSpecificforceであり、δＡ_ψはＡ_ψの座標変換に基づく誤差である。

また、式（４）の座標変換行列を変形し，δψが微小とすると式（５）を得る。

また、方位角誤差による加速度誤差を式（６）として表す。

さらに，bias誤差による加速度誤差を式（７）として表すと，式(６)と式(７)を加えた加速度誤差を改めてδＡ_ψとおき、更にScale factor の誤差を考慮すると式(８)として得られる。

（２）ロール角誤差による加速度誤差
この加速度誤差は同様に式（９）として得る．この場合はロール角誤差による重力加速度成分も加算される。

（３）ピッチ角誤差による加速度誤差
この加速度誤差も同様に式（１０）として得る．この場合はピッチ角誤差による重力加速度成分も加算される。

姿勢角誤差
式（２）からδA＾を推定した後，姿勢角誤差(δψ，δφ，δθ)を求めるには下記の要領で行う。
（１）図２（Ａ）に示すように、ＧＰＳから求めた推定加速度Ａ_{Y Z}（ベクトル要素：Ａ^＾ _Y，Ａ^＾ _Z）と推定加速度誤差δＡ_{Y Z}（ベクトル要素：δＡ^＾ _Y，δＡ^＾ _Z）の関係から姿勢角誤差δφ＝ｔａｎ^-1｜δＡ_YZ｜／｜Ａ_YZ｜が求まる。（図２（Ａ））。
（２）又、図２(Ｂ)に示すように、ＧＰＳから求めた推定加速度Ａ_XZ（ベクトル要素：Ａ^＾ _X，Ａ^＾ _Z）と推定加速度誤差δＡ_XZ（ベクトル要素：δＡ^＾ _X，δＡ^＾ _Z）の関係から、姿勢角誤差δθ＝ｔａｎ^-1｜δＡ_XZ｜／｜Ａ_XZ｜が求まる。（図２（Ｂ））。

（３）更に、図２（Ｃ）に示すように、ＧＰＳから求めた推定加速度Ａ_XY（ベクトル要素：Ａ^＾ _X，Ａ^＾ _Y）と推定加速度誤差δＡ_XY（ベクトル要素：δＡ^＾ _X，δＡ^＾ _Y）の関係から、方位角誤差δψ＝ｔａｎ^-1｜δＡ_XY｜／｜Ａ_XY｜が求まる。

また、前記で求めた姿勢角誤差δφ、δθ、δψに替えて、ビークル座標系においては下記のように姿勢角誤差δφ、δθ、δψを求めることもできる。
ここでδψの場合について説明すると、δＡを式（１１）等によってビークル座標系に変換する。

ビークル座標系の推定加速度を（a^＾ _x，a^＾ _y，a^＾ _z）とし、推定加速度誤差を（δa^＾ _x，δa^＾ _y，δa^＾ _z）とすると、推定加速度a_{Y Z}（ベクトル要素：ａ^＾ _Y，ａ^＾ _Z）と推定加速度誤差δa_{Y Z}（ベクトル要素：δａ^＾ _Y，δａ^＾ _Z）の関係から姿勢角誤差δφ＝ｔａｎ^-1｜δa_YZ｜／｜a_YZ｜が求まる。
又、推定加速度a_XZ（ベクトル要素：a^＾ _X，a^＾ _Z）と推定加速度誤差δa_XZ（ベクトル要素：δa^＾ _X，δa^＾ _Z）の関係から、姿勢角誤差δθ＝ｔａｎ^-1｜δa_XZ｜／｜a_XZ｜が求まる。
更に、推定加速度a_XY（ベクトル要素：a^＾ _X，a^＾ _Y）と推定加速度誤差δa_XY（ベクトル要素：δa^＾ _X，δa^＾ _Y）の関係から、方位角誤差δψ＝ｔａｎ^-1｜δa_XY｜／｜a_XY｜が求まる。

尚、加速度誤差を構成する（ａ）バイアスエラー、（ｂ）スケールファクターエラー、（ｃ）姿勢角誤差による加速度誤差、の要因が、姿勢角誤差に大きく影響する場合には、前記（ａ）〜（ｃ）を適当な方法で分離し、姿勢各誤差による加速度誤差のみを抽出する必要がある。

（４）次に、前記で求めた姿勢角誤差δφ、δθ、δψの極性を判定する。
姿勢角誤差δφの極性は推定加速度Ａ_YZと推定加速度誤差δＡ_YZの外積により、δθの極性は推定加速度Ａ_XZと推定加速度誤差δＡ_XZの外積による。又、δψの極性は推定加速度Ａ_XYと推定加速度誤差δＡ_XYの外積による。
あるいは、前記ビークル座標系においては、姿勢角誤差δφの極性は推定加速度a_{Y Z}と推定加速度誤差δa_YZの外積により、δθの極性は推定加速度a_XZと推定加速度誤差δa_XZの外積による。又、δψの極性は推定加速度a_XYと推定加速度誤差δa_XYの外積による。
（５）そして、時刻ｔ_ｉにおけるｘ軸回りのロール角φ、ｙ軸回りのピッチ角θ、ｚ軸回りのヨー角ψは、前記極性を考慮した姿勢角誤差δφ、δθ、δψの用いて、下記の算出式で求める。
推定ロール角φ＝（誤差を含んだロール角φ^〜）−（ロール姿勢角誤差δφ）
推定ピッチ角θ＝（誤差を含んだピッチ角θ^〜）−（ピッチ姿勢角誤差δθ）
推定ヨー角ψ ＝（誤差を含んだヨー角ψ^〜） −（方位角誤差δψ）

以上のように、時刻ｔ_ｉにおける推定ロール角φ、推定ピッチ角θ、推定ヨー角ψが求まり、順次、時刻ｔ_i＋δ、ｔ_i+２δ、・・・において姿勢角を求める。この求める方法は、１台のＧＰＳと、１台のジャイロ（３軸角速度センサ）と１台の３軸加速度センサ可能であり、且つ、精度のよい姿勢角を得ることができる。
尚、前記ではビークルに搭載の３軸加速度センサでビークルの観測加速度（ａ^〜 _x，ａ^〜 _y，ａ^〜 _z）を求めているが、速度センサを搭載して、この速度から加速度を求めてもよい。

Claims

ビークル（移動体）にＧＰＳとジャイロと加速度センサを搭載してビークルの姿勢角推定方法であって、
前記ＧＰＳによってビークルの位置（Ｒ_X，Ｒ_Y，Ｒ_Z）、前記ジャイロによってｘ軸回りのロール角速度ｐ^〜、ｙ軸回りのピッチ角速度ｑ^〜、ｚ軸回りのヨー角速度ｒ^〜、前記加速度センサによってｘ軸方向の縦加速度ａ^〜 _x、ｙ軸方向の横加速度ａ^〜 _y、ｚ軸方向の垂直加速度ａ^〜 _zが測定され、
（１）前記ＧＰＳによるビークルの位置（Ｒ_X，Ｒ_Y，Ｒ_Z）から地球座標系の推定加速度（Ａ^_X，Ａ^_Y，Ａ^_Z）を求め、
（２）前記加速度センサによるビークルの観測加速度（ａ^〜 _x，ａ^〜 _y，ａ^〜 _z）を求め、
（３）前記ジャイロで測定の角速度（ｐ^〜，ｑ^〜，ｒ^〜）から姿勢角誤差を含んだロール角φ^〜、ピッチ角θ^〜及びヨー角ψ^〜を求め、それらの値から姿勢角誤差を含んだ座標変換行列Ｅ^-1を求め、
（４）ビークル座標系の観測加速度（ａ^〜 _x，ａ^〜 _y，ａ^〜 _z）から姿勢角誤差を含んだ前記座標変換行列Ｅ^-1によって地球座標系の加速度（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _z）を求め、
（５）前記ＧＰＳから求めた地球座標系の推定加速度（Ａ^_X，Ａ^_Y，Ａ^_Z）と前記加速度センサから求めた地球座標系の観測加速度（Ａ^〜 _X，Ａ^〜 _Y，Ａ^〜 _z）の差を推定加速度誤差（δＡ＾_X，δＡ＾_Y，δＡ＾_Z）とし、
（６）ＧＰＳから求めた推定加速度Ａ_YZ（ベクトル要素：Ａ^_Y，Ａ^_Z）と推定加速度誤差δＡ_YZ（ベクトル要素：δＡ^_Y，δＡ^_Z）の関係から姿勢角誤差δφ＝ｔａｎ^-1｜δＡ_YZ｜／｜Ａ_YZ｜、推定加速度Ａ_XZ（ベクトル要素：Ａ^_X，Ａ^_Z）と推定加速度誤差δＡ_XZ（ベクトル要素：δＡ^_X，δＡ^_Z）の関係から姿勢角誤差δθ＝ｔａｎ^-1｜δＡ_XZ｜／｜Ａ_XZ｜、推定加速度Ａ_XY（ベクトル要素：Ａ^_X，Ａ^_Y）と推定加速度誤差δＡ_XY（ベクトル要素：δＡ^_X，δＡ^_Y）の関係から方位角誤差δψ＝ｔａｎ^-1｜δＡ_XY｜／｜Ａ_XY｜をそれぞれ求めると共に、前記推定加速度Ａ_YZと推定加速度誤差δＡ_YZの外積によりδφ、推定加速度Ａ_XZと推定加速度誤差δＡ_XZの外積によりδθ、推定加速度Ａ_XYと推定加速度誤差δＡ_XYの外積によりδψの極性判定を行うか、
あるいは、ビークル座標系の推定加速度ａ_yz（ベクトル要素：a^_y，a^_z）と加速度差δa_yz
（ベクトル要素：δa^_y，δa^_z）の関係から姿勢角誤差δφ＝ｔａｎ^-1｜δａ_yz｜／｜ａ_yz｜，推定加速度a_xz（ベクトル要素：a^_x，a^_z）と加速度誤差δa_xz（ベクトル要素：δa^_x，δa^_z）の関係から姿勢角誤差δθ＝ｔａｎ^-1｜δａ_xz｜／｜ａ_xz｜，推定加速度a_xy（ベクトル要素：a^_x，a^_y）と加速度誤差δa_xy（ベクトル要素：δa^_x，δa^_y）の関係から姿勢角誤差δψ＝ｔａｎ^-1｜δａ_xy｜／｜ａ_xy｜をそれぞれ求めると共に、前記推定加速度a_YZと推定加速度誤差δa_YZの外積によりδφ、推定加速度a_XZと推定加速度誤差δa_XZの外積によりδθ、推定加速度a_XYと推定加速度誤差δa_XYの外積によりδψの極性判定を行い、
（７）前記極性を考慮した姿勢角誤差δφ、δθ、δψにより下記の算出式で推定姿勢角（ロール角φ、ピッチ角θ、ヨー角ψ）を求める。
推定ロール角φ＝（（誤差を含んだロール角φ^〜）−（ロール姿勢角誤差δφ）
推定ピッチ角θ＝（誤差を含んだピッチ角θ^〜）−（ピッチ姿勢角誤差δθ）
推定ヨー角ψ ＝（誤差を含んだヨー角ψ^〜） −（方位角誤差δψ）