JPH04370712A - 慣性装置 - Google Patents

Abstract

(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】この発明は外部の基準となる慣性
航法装置（以下外部ＩＮＳと言う）より姿勢角、方位角
信号を入力して、自身の取付角度の外部ＩＮＳの取付角
度に対する偏差（取付けミスアライメントと言う）を計
算し、それら偏差データに基づいて自身が搭載されてい
る機体の姿勢角、方位角を計算して、外部に出力する慣
性航法装置（以下ＩＮＳと言う）に関する。

【０００２】

【従来の技術】この種のＩＮＳ１は例えば航空機に搭載
される航走体（例えばミサイル）に内蔵されるものであ
り、外部ＩＮＳ２はその航空機が有するＩＮＳである。 従来のＩＮＳ１を図４を参照して説明する。外部ＩＮＳ
２は入力角速度ωより自身に固有の入力軸Ｘ，Ｙ，Ｚ周
りの回転角速度ωＸ，ωＹ，ωＺを検出し、またこれら
の検出値を基に自身のロール角φＲ　、ピッチ角θＲ及
び方位角ψＲ　を演算する。なおロール角及びピッチ角
はまとめて姿勢角とも言われる。前記角速度ωＸ，ωＹ
，ωＺはＩＮＳ１の角速度差値演算部３に、また前記姿
勢角φＲ　，θＲ　、方位角ψＲ　はＩＮＳ１の補正演
算部４に入力される。ＩＮＳ１に内蔵されるジャイロス
コープ（以下単にジャイロと言う）５は、入力角速度ω
より自身に固有の入力軸（ＩＮＳ１の入力軸とも言う）
ｘ，ｙ，ｚの各軸周りの回転角速度ωｘ，ωｙ，ωｚを
検出し、角速度差値演算部３及び取付けミスアライメン
ト推定演算部（以下カルマンフィルタと言う）６に供給
する。角速度差値演算部３は、ジャイロ５の検出した角
速度より外部ＩＮＳ２の検出した角速度を減算して偏差
値　　δωｘ＝ωｘ−ωＸ，δωｙ＝ωｙ−ωＹ，δω
ｚ＝ωｚ−ωＺ　　　　（１）を演算し、カルマンフィ
ルタ６に入力する。

【０００３】カルマンフィルタ６はこれらの偏差値とジ
ャイロ５より入力された角速度ωｘ，ωｙ，ωｚとより
、図５に示すようなＩＮＳ１のＸ，Ｙ，Ｚ軸に対する取
付けミスアライメントφＸ，φＹ，φＺを演算し補正演
算部４に供給する。補正演算部４はこれらのミスアライ
メントと外部ＩＮＳ２のロール角φＲ，ピッチ角θＲ　
，方位角ψＲ　とよりＩＮＳ１のロール角φ，ピッチ角
θ，方位角ψを計算して外部に出力する。

【０００４】次に、公知のカルマンフィルタの要点を述
べる。あるシステムにおいて、推定しようとするｎ行１
列のデータ（ステートベクトル）〔Ｘ〕（〔〕はマトリ
ックスを表わす。以下同様）が 　　　　　　　　ｄ〔Ｘ〕／ｄｔ＝〔Ｆ〕〔Ｘ〕＋〔Ｕ
〕　　　　　　　　　　　　　　　　　　　　（２）で
表わされる微分方程式でモデル化でき、かつ〔Ｘ〕に何
らかの係数〔Ｈ〕を掛けた量〔Ｚ〕、即ち　　　　　　
　　〔Ｚ〕＝〔Ｈ〕〔Ｘ〕＋〔Ｖ〕　　　　　　　　　
　　　　　　　　　　　　　　　　　　　（３）が実際
のシステムにおいて観測できる場合に、カルマンフィル
タは以下のようにしてデータ〔Ｘ〕の推定データ〔Ξ〕
（ｎ行１列）を演算する。なお、前記〔Ｈ〕（ｍ行ｎ列
）を観測行列、〔Ｚ〕（ｍ行１列）を観測ベクトル、〔
Ｕ〕（ｎ行１列）をシステムノイズベクトル、〔Ｆ〕（
ｎ行ｎ列）をシステム行列、〔Ｖ〕（ｍ行１列）を観測
ノイズベクトルと言う。

【０００５】演算は周期Δｔ毎に離散的に行ない、推定
データ〔Ξ〕及びそのコバリアンス（共分散）行列〔Ｐ
〕（ｎ行ｎ列）を次のように更新する。 　　　　　　　　〔Ξ〕ｋ　−　＝〔Φ〕ｋ−１　〔Ξ
〕ｋ−１　＋　　　　　　　　　　　　　　　　　　　
　　　　（４）　　　　　　　　〔Ｐ〕ｋ　−　＝〔Φ
〕ｋ−１　〔Ｐ〕ｋ−１　＋〔Φ〕ｋ−１　Ｔ　＋〔Ｑ
〕ｋ−１（５）〔Φ〕は状態遷移行列（ｎ行ｎ列）で、
〔Φ〕＝ｅｘｐ（〔Ｆ〕Δｔ）である。また〔Ｑ〕（ｎ
行ｎ列）は〔Ｕ〕のコバリアンス（共分散）行列である
。添字ｋはｋ回目の計算値を意味する。観測が行われる
と、カルマンゲイン〔Ｋ〕を 〔Ｋ〕ｋ　＝〔Ｐ〕ｋ　−〔Ｈｋ　〕Ｔ　　　　　　　
　　　　（〔Ｈ〕ｋ　〔Ｐ〕ｋ　−〔Ｈ〕ｋ　Ｔ　＋〔
Ｒ〕ｋ　）−１　　　　　　　　　　　（６）により計
算し、ステートベクトルの推定量〔Ξ〕及びそのコバリ
アンス〔Ｐ〕を次のように修正する。 〔Ξ〕ｋ　＋　＝〔Ξ〕ｋ　−　＋〔Ｋ〕ｋ　（〔Ｚ〕
ｋ　−〔Ｈ〕ｋ　〔Ξ〕ｋ　−　）　　（７）〔Ｐ〕ｋ
　＋　＝（〔１〕−〔Ｋ〕ｋ　〔Ｈ〕ｋ　）〔Ｐ〕ｋ　
−　　　　　　　　　　　　　（〔１〕−〔Ｋ〕ｋ　〔
Ｈ〕ｋ　）Ｔ　＋〔Ｋ〕ｋ　〔Ｒ〕ｋ　〔Ｋ〕ｋ　Ｔ　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　　　　　　　　　　　（８）なお指標の「−」
はカルマンゲインにより修正計算する前の値を、「＋」
は修正計算後の値を意味し、「Ｔ」は転置行列を意味す
る。このような修正演算を周期Δｔ毎に繰返すことによ
り、推定データ〔Ξ〕の値は、実際のステートベクトル
〔Ｘ〕の値により近付く。このようにして〔Ｘ〕の推定
データ〔Ξ〕が得られる。

【０００６】従来のＩＮＳでは 　　　　　　　　〔Ｘ〕＝〔ｘ１１ｘ２１ｘ３１〕Ｔ　
＝〔φＸφＹφＺ〕Ｔ　　　　　　　　　　　（９）と
し、（２）式に対応するシステムのモデルを　　　　　
　　　ｄ〔φＸφＹφＺ〕Ｔ　／ｄｔ＝

〔０〕〔φＸφ
ＹφＺ〕Ｔ　　　　　　　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　＋〔ｕＸｕＹｕＺ
〕Ｔ　　　　　　　　　（１０）で表わす。また（３）
式の観測行列〔Ｈ〕の要素ｈｉｊを回転角速度ωｘ，ω
ｙ，ωｚを用いて、　　　　　　　　〔ｈ１１ｈ１２ｈ
１３〕＝〔０，−ωｚ，ωｙ〕；　　　　　　　　〔ｈ
２１ｈ２２ｈ２３〕＝〔ωｚ，０，−ωｘ〕；　　　　
　　　　〔ｈ３１ｈ３２ｈ３３〕＝〔−ωｙ，ωｘ，０
〕　　　　　　　　　　　　　　　　　　（１１）と表
わし、観測ベクトル〔Ｚ〕を 　　　　　　　　〔Ｚ〕＝〔ｚ１１ｚ２１ｚ３１〕Ｔ　
＝〔δωｘδωｙδωｚ〕Ｔ　　　　　（１２）として
、前述のカルマンフィルタによる演算により取付ミスア
ライメントφＸ，φＹ，φＺの推定値を求めている。な
お（１２）式のδωｘ，δωｙ，δωｚは（１）式で示
したものである。

【０００７】

【発明が解決しようとする課題】従来のＩＮＳ１では、
自身のジャイロ５で検出した角速度信号ωｘ，ωｙ，ω
ｚを外部ＩＮＳ２の基準角速度信号ωＸ，ωＹ，ωＺと
直接比較して偏差値δωｘ，δωｙ，δωｚを求め、こ
れら偏差値より取付けミスアライメントφＸ，φＹ，φ
Ｚを推定しているために、角速度信号ωｘ，ωｙ，ωｚ
或いは基準角速度信号ωＸ，ωＹ，ωＺに重畳するノイ
ズが多いと、取付けミスアライメントφＸ，φＹ，φＺ
の計算精度が低下し、姿勢角φ，θ；方位角ψの誤差が
増大する欠点があった。この発明の目的は、これら従来
の欠点を解決し、前記角速度信号のノイズの影響を軽減
して、姿勢角φ，θ；方位角ψの精度を向上させようと
するものである。

【０００８】

【課題を解決するための手段】この発明は基準方向余弦
行列演算部と、ジャイロスコープと、方向余弦行列演算
部と、角度誤差演算部と、回転角度演算部と、カルマン
フィルタと、補正演算部とを具備する慣性装置に関する
ものである。前記基準方向余弦行列演算部は、基準とな
る外部ＩＮＳより機体のロール角φＲ，ピッチ角θＲ　
及び方位角ψＲ　を入力して、基準方向余弦行列〔ＣＲ
　〕を演算して、前記方向余弦行列演算部及び前記角度
誤差演算部に供給するものである。

【０００９】前記ジャイロスコープは、自身に固有の、
互いに直交する入力軸ｘ，ｙ，ｚの周りの回転角速度ω
ｘ，ωｙ，ωｚを計測して、前記方向余弦行列演算部及
び前記回転角度演算部に供給するものである。前記方向
余弦行列演算部は、所定の周期Ｔで前記基準方向余弦行
列〔ＣＲ　〕で初期化した後、前記回転角速度ωｘ，ω
ｙ，ωｚを用いて方向余弦行列〔Ｃ〕を演算して、前記
角度誤差演算部に供給するものである。

【００１０】前記角度誤差演算部は、前記方向余弦行列
演算部の前記初期化する直前の時点の前記方向余弦行列
〔Ｃ〕と、同じ時点の前記基準方向余弦行列とを前記周
期Ｔでサンプリングして、基準回転角度θＸ，θＹ，θ
Ｚ（前記外部ＩＮＳの計測値に基づく、外部ＩＮＳの入
力軸Ｘ，Ｙ，Ｚの周りの回転角度）に対する回転角度θ
ｘ，θｙ，θｚ（前記ジャイロスコープの計測値に基づ
く前記入力軸ｘ，ｙ，ｚの周りの回転角度）の偏差δθ
ｘ，δθｙ，δθｚを演算して、前記カルマンフィルタ
に供給するものである。

【００１１】前記回転角度演算部は、前記方向余弦行列
演算部の前記初期化と同じタイミングで自身の出力デー
タをゼロにリセットした後、前記回転角速度ωｘ，ωｙ
，ωｚを積分して回転角度θｘ，θｙ，θｚを演算し、
前記カルマンフィルタに供給するものである。前記カル
マンフィルタは、前記回転角度θｘ，θｙ，θｚと、前
記回転角度偏差δθｘ，δθｙ，δθｚとを用いて、前
記外部ＩＮＳに対する取付けミスアライメントφＸ，φ
Ｙ，φＺを演算して、前記補正演算部に供給するもので
ある。

【００１２】前記補正演算部は、基準となる前記ロール
角φＲ　，ピッチ角θＲ　及び方位角ψＲ　と、前記取
付けミスアライメントφＸ，φＹ，φＺとを用いて機体
のロール角φ，ピッチ角θ及び方位角ψを演算して外部
に出力するものである。

【００１３】

【実施例】この発明の実施例を図１を参照して説明する
。図１には図４と対応する部分に同じ符号を付し、重複
説明を省略する。方向余弦行列演算部１１はジャイロ５
より入力される回転角速度ωｘ，ωｙ，ωｚを用いて方
向余弦行列〔Ｃ〕を演算して角度誤差演算部１２に供給
する。

【００１４】方向余弦行列は二つの直交座標系の関係を
表わす行列で、例えばＮ（Ｎｏｒｔｈ），Ｅ（Ｅａｓｔ
），Ｄ（Ｄｏｗｎ）という直交座標軸とｘ，ｙ，ｚとい
う直交座軸とのなす角を図２に示すように、α１　，α
２　，α３　；β１　，β２　，β３　；γ１　，γ２
　，γ３　で表わすと、方向余弦行列〔Ｃ〕のｉ行ｊ列
の要素をｃｉｊとすれば 　　　　〔ｃ１１ｃ１２ｃ１３〕＝〔ｃｏｓα１　，ｃ
ｏｓβ１　，ｃｏｓγ１　〕；　　　　〔ｃ２１ｃ２２
ｃ２３〕＝〔ｃｏｓα２　，ｃｏｓβ２　，ｃｏｓγ２
　〕；　　　　〔ｃ３１ｃ３２ｃ３３〕＝〔ｃｏｓα２
　，ｃｏｓβ３　，ｃｏｓγ３　〕　　　　（１３）で
与えられる。

【００１５】方向余弦行列〔Ｃ〕と角速度行列〔Ω〕の
関係は、よく知られているように 　　　　　　　　ｄ〔Ｃ〕／ｄｔ＝〔Ｃ〕〔Ω〕　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
（１４）の微分方程式で表わされる。ここで角速度行列
〔Ω〕はそのｉ行ｊ列の要素をωｉｊとすれば、　　　
　　　　　〔ω１１ω１２ω１３〕＝〔０，−ωｚ，ω
ｙ〕，　　　　　　　　〔ω２１ω２２ω２３〕＝〔ω
ｚ，０，−ωｘ〕，　　　　　　　　〔ω３１ω３２ω
３３〕＝〔−ωｙ，ωｘ，０〕　　　　　　　　　　　
　　　　　　　（１５）で与えられる。

【００１６】方向余弦行列演算部１１は、次式により方
向余弦行列〔Ｃ〕ｎ＋１　を計算する。 　　　　〔Ｃ〕ｎ＋１　＝〔Ｃ〕ｎ　（〔１〕＋〔Ω〕
ΔＴ＋（１／２）〔Ω〕２　ΔＴ２　＋　　　　　　　
　　　　　　　　　（１／６）〔Ω〕３　ΔＴ３　＋　
　…）　　　　　　　　　　　　　　　　（１６）上式
で、ΔＴは計算周期であり、〔１〕は単位マトリクスで
ある。添字ｎはｎ回目の計算値を意味する。

【００１７】回転角度演算部１３は図３Ｂに示す周期Ｔ
のクロックＣＫ２　のタイミングで出力値をゼロにリセ
ットした後、ジャイロ５より入力される角速度ωｘ，ω
ｙ，ωｚを用いて次式により回転角度θｘ，θｙ，θｚ
を演算してカルマンフィルタ６に供給する（図３Ｄ）。 　　θｘ＝∫ωｘｄｔ，θｙ＝∫ωｙｄｔ，θｚ＝∫ω
ｚｄｔ　　　　　　　　　　（１７）基準方向余弦行列
演算部１４は外部ＩＮＳ２より入力される基準姿勢角φ
Ｒ，θＲ　及び方位角ψＲ　を用いて次式により基準方
向余弦行列〔ＣＲ　〕を演算して角度誤差演算部１２に
供給する。

【００１８】ｃＲ１１　＝ｃｏｓθＲ　ｃｏｓψＲ　，
ｃＲ１２　＝−ｃｏｓφＲ　ｓｉｎψＲ　＋ｓｉｎφＲ
　ｓｉｎθＲ　ｃｏｓψＲ　， ｃＲ１３　＝ｓｉｎφＲ　ｓｉｎψＲ　＋ｃｏｓφＲ　
ｓｉｎθＲ　ｃｏｓψＲ　， ｃＲ２１　＝ｃｏｓθＲ　ｓｉｎψＲ　，ｃＲ２２　＝
ｃｏｓφＲ　ｃｏｓψＲ　＋ｓｉｎφＲ　ｓｉｎθＲ　
ｓｉｎψＲ　， ｃＲ２３　＝−ｓｉｎφＲ　ｃｏｓψＲ　＋ｃｏｓφＲ
　ｓｉｎθＲ　ｓｉｎψＲ　， ｃＲ３１　＝−ｓｉｎθＲ　， ｃＲ３２　＝ｓｉｎφＲ　ｃｏｓθＲ　，　　ｃＲ３３
　＝ｃｏｓφＲ　ｃｏｓθＲ　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　（１
８）上式では〔ＣＲ　〕のｉ行ｊ列の要素をｃＲｉｊ　
で表わしている。

【００１９】上記ｃＲｉｊ　と方向余弦行列〔Ｃ〕の要
素ｃｉｊの時間に対する変化特性を図３Ｃに示す。ｃｉ
ｊはクロックＣＫ２　でｃＲｉｊ　と等しい値に初期化
される。角度誤差演算部１２は、方向余弦行列演算部１
１が初期化した時点よりほゞＴ時間経過し次の初期化を
行う直前の時点の方向余弦行列〔Ｃ〕及び同じ時点の基
準方向余弦行列〔ＣＲ　〕を図３Ａの周期Ｔのクロック
信号ＣＫ１　のタイミングでサンプリングし、次式によ
り方向余弦誤差行列〔δＣ〕を演算する。

【００２０】 　　〔δＣ〕＝〔ＣＲ　〕Ｔ　〔Ｃ〕　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　（１９）（１９）式を各行列の要素で表わすと δｃ１１＝ｃＲ１１　ｃ１１＋ｃＲ２１　ｃ２１＋ｃＲ
３１　ｃ３１，δｃ１２＝ｃＲ１１　ｃ１２＋ｃＲ２１
　ｃ２２＋ｃＲ３１　ｃ３２，δｃ１３＝ｃＲ１１　ｃ
１３＋ｃＲ２１　ｃ２３＋ｃＲ３１　ｃ３３，δｃ２１
＝ｃＲ１２　ｃ１１＋ｃＲ２２　ｃ２１＋ｃＲ３２　ｃ
３１，δｃ２２＝ｃＲ１２　ｃ１２＋ｃＲ２２　ｃ２２
＋ｃＲ３２　ｃ３２，δｃ２３＝ｃＲ１２　ｃ１３＋ｃ
Ｒ２２　ｃ２３＋ｃＲ３２　ｃ３３，δｃ３１＝ｃＲ１
３　ｃ１１＋ｃＲ２３　ｃ２１＋ｃＲ３３　ｃ３１，δ
ｃ３２＝ｃＲ１３　ｃ１２＋ｃＲ２３　ｃ２２＋ｃＲ３
３　ｃ３２，　　δｃ３３＝ｃＲ１３　ｃ１３＋ｃＲ２
３　ｃ２３＋ｃＲ３３　ｃ３３，　　　　　　　　　　
　　　　　　　　（２０）いま、外部ＩＮＳ２の出力す
るφＲ　，θＲ　，ψＲ　に基づく（基準方向余弦行列
〔ＣＲ　〕に対応する）基準入力軸Ｘ，Ｙ，Ｚ周りの回
転角度をθＸ，θＹ，θＺとし、ジャイロ５の出力する
回転角速度ωｘ，ωｙ，ωｚに基づく（方向余弦行列〔
Ｃ〕に対応する）ｘ，ｙ，ｚ軸周りの回転角度をθｘ，
θｙ，θｚとし、前者に対する後者の偏差をδθｘ，δ
θｙ，δθｚとする。即ち、　　δθｘ＝θｘ−θＸ，
δθｙ＝θｙ−θＹ，δθｚ＝θｚ−θＺ　　　　（２
１）角度誤差演算部１２は次式よりこれらの角度偏差δ
θｘ，δθｙ，δθｚを演算してカルマンフィルタ６に
供給する。

【００２１】δθｘ＝（δｃ３２−δｃ２３）／２，δ
θｙ＝（δｃ１３−δｃ３１）／２，　　δθｚ＝（δ
ｃ２１−δｃ１２）／２　　　　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　（２２）カルマ
ンフィルタ６は、（２２）式の回転角度誤差δθｘ，δ
θｙ，δθｚと（１７）式の回転角度θｘ，θｙ，θｚ
を、図３ＡのクロックＣＫ１　のタイミングでサンプリ
ングし、それぞれのデータを図４の従来例に関して述べ
た角速度誤差δωｘ，δωｙ，δωｚ及び角速度ωｘ，
ωｙ，ωｚの代りに用いて、従来例で説明したのと同様
な演算により取付けミスアライメントφＸ，φＹ，φＺ
の推定値を求めて補正演算部４に供給する。

【００２２】いま、ＩＮＳ１の正しい方向余弦行列〔Ｃ
０　〕を次のように定義する。 　　〔ＣＯ　〕＝〔ＣＲ　〕〔ΔＣ〕　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　（２３）上式で〔ＣＲ　〕は基準方向余弦行列で
あり、〔ΔＣ〕は　　〔第１行〕＝〔１，−φＺ，φＹ
〕；　　〔第２行〕＝〔φＺ，１，−φＸ〕；　　〔第
３行〕＝〔−φＹ，φＸ，１〕　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　（２４）で与
えられる行列である。補正演算部４は〔ＣＯ　〕の要素
ｃｏｉｊ　よりロール角φ，ピッチ角θ，方位角ψを次
式により演算して外部に出力する。

【００２３】 　　φ＝ｔａｎ−１（ｃ０３２　／ｃ０３３　）　　　
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　（２５）　　θ＝ｓｉｎ−１（−ｃ０３１　）
　　　　　　　　　　　　　　　　　　　　　　　　　
　　　　　　　　　　　　　（２６）　　ψ＝ｔａｎ−
１（ｃ０２１　／ｃ０１１　）　　　　　　　　　　　
　　　　　　　　　　　　　　　　　　　　　（２７）
図１では基準方向余弦行列演算部１４をＩＮＳ１に内蔵
したが、この発明はこの場合に限らず、ＩＮＳ１の外部
に設けてもよい。

【００２４】

【発明の効果】以上述べたように、この発明は、慣性装
置の取付けミスアライメントを計算するのに、角速度信
号を直接使用せず、角速度信号を積分して求める角度デ
ータを使用することにより、角速度ノイズ成分が平均化
され取付けミスアライメントを精度良く求めることがで
きる。従って姿勢角φ，θ方位角ψに対するノイズの影
響が軽減され、その精度を大幅に向上できる。

【００２５】また、角速度を用いて計算する従来の装置
では、角速度変化に対して十分早い周期でミスアライメ
ント計算のためのカルマンフィルタ処理を行なう必要が
あったが、本発明による装置では、角速度の変化は積分
されてより緩やかに変化する回転角度を用いるため、ミ
スアライメント計算のためのカルマンフィルタ処理を遅
い周期で行なえばよく、コンピュータに対する負荷を軽
減できる。

【００２６】従来の装置では、外部角速度信号を必要と
するため、基準とする外部慣性航法装置には、角速度信
号も供給できるストラップダウン型の慣性航法装置を必
要としたが、本発明の装置では、姿勢角、方位角信号さ
え供給できればよくストラップダウン型の他、プラット
フォーム型の慣性航法装置を利用することができ、はな
はだ便利である。

【図面の簡単な説明】

【図１】この発明の実施例を示すブロック図。

【図２】原点を共有する二つの直交座標系間で定義され
る方向余弦行列を説明するための図。

【図３】図１の要部の波形図。

【図４】従来のＩＮＳのブロック図。

【図５】外部ＩＮＳ２の角速度入力軸Ｘ，Ｙ，Ｚと、Ｉ
ＮＳ１の角速度入力軸ｘ，ｙ，ｚと、ＩＮＳ１の外部Ｉ
ＮＳ２を基準とした取付けミスアライメントφＸ，φＹ
，φＺとを示す図。

Claims (1)

【特許請求の範囲】
1. 【請求項１】　　基準方向余弦行列演算部と、ジャイロ
   スコープと、方向余弦行列演算部と、角度誤差演算部と
   、回転角度演算部と、カルマンフィルタと、補正演算部
   とを具備する慣性装置であって、前記基準方向余弦行列
   演算部は、基準となる外部ＩＮＳより機体のロール角φ
   Ｒ　，ピッチ角θＲ　及び方位角ψＲ　を入力して、基
   準方向余弦行列〔ＣＲ　〕を演算して、前記方向余弦行
   列演算部及び前記角度誤差演算部に供給するものであり
   、前記ジャイロスコープは、自身に固有の、互いに直交
   する入力軸ｘ，ｙ，ｚの周りの回転角速度ωｘ，ωｙ，
   ωｚを計測して、前記方向余弦行列演部及び前記回転角
   度演算部に供給するものであり、前記方向余弦行列演算
   部は、所定の周期Ｔで前記基準方向余弦行列〔ＣＲ　〕
   で初期化した後、前記回転角速度ωｘ，ωｙ，ωｚを用
   いて方向余弦行列〔Ｃ〕を演算して、前記角度誤差演算
   部に供給するものであり、前記角度誤差演算部は、前記
   方向余弦行列演算部の前記初期化する直前の時点の前記
   方向余弦行列〔Ｃ〕と、同じ時点の前記基準方向余弦行
   列とを前記周期Ｔでサンプリングして、基準回転角度θ
   Ｘ，θＹ，θＺ（前記外部ＩＮＳの計測値に基づく、外
   部ＩＮＳの入力軸Ｘ，Ｙ，Ｚの周りの回転角度）に対す
   る回転角度θｘ，θｙ，θｚ（前記ジャイロスコープの
   計測値に基づく、前記入力軸ｘ，ｙ，ｚの周りの回転角
   度）の偏差δθｘ，δθｙ，δθｚを演算して、前記カ
   ルマンフィルタに供給するものであり、前記回転角度演
   算部は、前記方向余弦行列演算部の前記初期化と同じタ
   イミングで自身の出力データをゼロにリセットした後、
   前記回転角速度ωｘ，ωｙ，ωｚを積分して回転角度θ
   ｘ，θｙ，θｚを演算し、前記カルマンフィルタに供給
   するものであり、前記カルマンフィルタは、前記回転角
   度θｘ，θｙ，θｚと、前記回転角度偏差δθｘ，δθ
   ｙ，δθｚとを用いて、前記外部ＩＮＳに対する取付け
   ミスアライメントφＸ，φＹ，φＺを演算して、前記補
   正演算部に供給するものであり、前記補正演算部は、基
   準となる前記ロール角φＲ　，ピッチ角θＲ　及び方位
   角ψＲ　と、前記取付けミスアライメントφＸ，φＹ，
   φＺとを用いて機体のロール角φ，ピッチ角θ及び方位
   角ψを演算して外部に出力するものであることを特徴と
   する、慣性装置。