航法装置及び推定方法
【課題】航法装置の推定の精度を劣化させることなく、計算量を少なくする。
【解決手段】姿勢角外挿計算部１２２は、角速度センサ１１１が計測した移動体座標系における移動体８００の角速度ωｂに基づいて、慣性座標系における移動体８００の姿勢角ｑｎを計算する。速度増分計算部１２６は、加速度センサ１１２が計測した移動体座標系における移動体８００に加わる加速度ａｂと姿勢角ｑｎとに基づいて、慣性座標系における移動体８００の速度の増分Δｖｎを計算する。速度外挿計算部１２７は、速度の増分Δｖｎに基づいて、慣性座標系における移動体８００の速度ｖｎを計算する。速度減算部１３０は、速度ｖｎと、ＧＰＳ受信機１１３が計測した慣性座標系における移動体８００の速度ｖｇとの差ｄｖを計算する。第一推定部１４０は、姿勢角ｑｎと、速度の増分Δｖｎと、速度の差ｄｖとに基づいて、角速度の誤差ｂｇと、姿勢角の誤差εとを推定する。
【解決手段】姿勢角外挿計算部１２２は、角速度センサ１１１が計測した移動体座標系における移動体８００の角速度ωｂに基づいて、慣性座標系における移動体８００の姿勢角ｑｎを計算する。速度増分計算部１２６は、加速度センサ１１２が計測した移動体座標系における移動体８００に加わる加速度ａｂと姿勢角ｑｎとに基づいて、慣性座標系における移動体８００の速度の増分Δｖｎを計算する。速度外挿計算部１２７は、速度の増分Δｖｎに基づいて、慣性座標系における移動体８００の速度ｖｎを計算する。速度減算部１３０は、速度ｖｎと、ＧＰＳ受信機１１３が計測した慣性座標系における移動体８００の速度ｖｇとの差ｄｖを計算する。第一推定部１４０は、姿勢角ｑｎと、速度の増分Δｖｎと、速度の差ｄｖとに基づいて、角速度の誤差ｂｇと、姿勢角の誤差εとを推定する。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、航空機等の移動体に搭載され、移動体の３次元姿勢角や速度の推定を行う航法装置及びその推定方法に関する。
【背景技術】
【０００２】
航法装置は、慣性センサである角速度センサおよび加速度センサの出力を積分することにより、移動体の姿勢角および速度を得る。特に、安定度の低い廉価な慣性センサを用いた廉価な航法装置の場合、出力に含まれるバイアス誤差等の誤差要因により、時間と共に急激に精度が劣化するため、ＧＰＳ受信機の位置または速度出力により、誤差を補正する機能を有している場合が多い。この誤差の補正手段としては、カルマンフィルタ等の推定器が用いられる。
【非特許文献１】廣川類、佐藤幸一、中久喜健司「Ｄｅｓｉｇｎ ａｎｄ Ｅｖａｌｕａｔｉｏｎ ｏｆ Ａ Ｔｉｇｈｔｌｙ Ｃｏｕｐｌｅｄ ＧＰＳ／ＩＮＳ ｕｓｉｎｇ Ｌｏｗ Ｃｏｓｔ ＭＥＭＳ ＩＭＵ」ＧＰＳ／ＧＮＳＳ国際シンポジウム２００３、２００３年。
【非特許文献２】Ｊａｙ Ａ．Ｆａｒｒｅｌｌ、Ｍａｔｔｈｅｗ Ｂａｒｔｈ「Ｔｈｅ Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ ａｎｄ Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ」ＭｃＧｒａｗ−Ｈｉｌｌ、１９９９年。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
航空機等、三次元空間で加速度運動をする移動体の場合、カルマンフィルタで高次の行列計算を多く行う必要があるため、計算負荷が高い。そのため、高い処理能力が必要となり、航法装置の製造コストが高くなるという課題がある。
この発明は、例えば、上記のような課題を解決するためになされたものであり、推定の精度を劣化させることなく、航法装置における計算負荷を低減することにより、航法装置の製造コストを低くすることを目的とする。
【課題を解決するための手段】
【０００４】
この発明にかかる航法装置は、
移動体に搭載される航法装置において、
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有し、
上記角速度計測部は、上記移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部は、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部は、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部は、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部は、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部は、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部は、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする。
【発明の効果】
【０００５】
この発明にかかる航法装置によれば、姿勢角外挿計算部が計算した姿勢角ｑｎと、速度増分計算部が計算した速度の増分Δｖｎと、速度減算部が計算した速度の差ｄｖとに基づいて、第一推定部が角速度の誤差ｂｇと、姿勢角の誤差εとを推定するので、推定にかかる計算量が少なく、航法装置１００の製造コストを低く抑えることができる。
【発明を実施するための最良の形態】
【０００６】
実施の形態１．
実施の形態１について、図１〜図８を用いて説明する。
【０００７】
図１は、この実施の形態における航法装置１００の機能ブロックの構成の一例を示すブロック構成図である。
航法装置１００は、航空機や車両などの移動体８００に搭載され、移動体８００の位置、速度、姿勢角などを推定する装置である。
航法装置１００は、角速度センサ１１１、加速度センサ１１２、ＧＰＳ受信機１１３、慣性航法部１２０、速度減算部１３０、第一推定部１４０、第二推定部１５０、姿勢角出力部１９１、速度出力部１９２、位置出力部１９３を有する。
【０００８】
なお、以下の説明において、移動体８００を基準とする移動体座標系（第一座標系）の直交軸を、大文字でＸ軸・Ｙ軸・Ｚ軸、地球を基準とする慣性座標系（第二座標系）の直交軸を小文字でｘ軸・ｙ軸・ｚ軸と表記する。
移動体座標系は、例えば、移動体８００の重心を原点とし、移動体８００の前方向をＸ軸、移動体８００の右方向をＹ軸、移動体８００の下方向をＺ軸とする。
慣性座標系は、例えば、地球上の所定の点を原点とし、北方向をｘ軸、東方向をｙ軸、下方向をｚ軸とする。あるいは、慣性座標系は、地球の中心を原点とし、春分点方向をｘ軸、赤道面内で春分点方向に対して垂直な方向をｙ軸、地軸方向をｚ軸としてもよい。
【０００９】
角速度センサ１１１（角速度計測部）は、例えば、ジャイロスコープなど、移動体８００の角速度を計測するセンサである。角速度センサ１１１は、Ｘ軸を中心とする回転の角速度ωＸと、Ｙ軸を中心とする回転の角速度ωＹと、Ｚ軸を中心とする回転の角速度ωＺとの三つの角速度を計測する。以下、角速度センサ１１１が計測した移動体座標系における三軸の角速度ωＸ，ωＹ，ωＺを成分とする三次の縦ベクトルを「ωｂ」（＝［ωＸ ωＹ ωＺ］Ｔ）と表記し、角速度センサ１１１が時刻ｔｋに計測した角速度ωｂを「ωｂｋ」と表記する。角速度センサ１１１は、計測した角速度ωｂを表わすデータを出力する。角速度センサ１１１が計測した角速度ωｂを表わすデータを出力する頻度は、例えば、１秒間に１００回程度である。
【００１０】
加速度センサ１１２（加速度計測部）は、移動体８００に加わる加速度を計測するセンサである。加速度センサ１１２は、Ｘ軸方向の加速度ａＸと、Ｙ軸方向の加速度ａＹと、Ｚ軸方向の加速度ａＺとの三つの加速度を計測する。以下、加速度センサ１１２が計測した移動対座標系における三軸の加速度ａＸ，ａＹ，ａＺを成分とする三次の縦ベクトルを「ａｂ」（＝［ａＸ ａＹ ａＺ］Ｔ）と表記し、加速度センサ１１２が時刻ｔｋに計測した加速度ａｂを「ａｂｋ」と表記する。加速度センサ１１２は、計測した加速度ａｂを表わすデータを出力する。加速度センサ１１２が計測した加速度ａｂを表わすデータを出力する頻度は、角速度センサ１１１と同様、例えば、１秒間に１００回程度である。
【００１１】
ＧＰＳ受信機１１３（速度計測部・位置計測部・速度計測精度算出部）は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星が送信した電波を受信することにより、移動体８００の位置ｐｇや速度ｖｇなどを計測する。ＧＰＳ受信機１１３が計測する位置ｐｇや速度ｖｇは、慣性座標系におけるものであり、それぞれｘｙｚ軸方向の成分を有する。以下の説明において、ＧＰＳ受信機１１３が計測した位置ｐｇや速度ｖｇは、三次の縦ベクトルとして扱う。また、ＧＰＳ受信機１１３が時刻ｔｋに計測した位置ｐｇ及び速度ｖｇをそれぞれ「ｐｇｋ」「ｖｇｋ」と表記する。ＧＰＳ受信機１１３は、計測した位置ｐｇや速度ｖｇを表わすデータを出力する。ＧＰＳ受信機１１３が計測した位置ｐｇや速度ｖｇを表わすデータを出力する頻度は、例えば、１秒間に１〜４回程度である。
また、ＧＰＳ受信機１１３は、受信した電波の数や、ＧＰＳ衛星の配置などに基づいて、計測した位置ｐｇや速度ｐｇの精度を推定する。ＧＰＳ受信機１１３は、推定した精度を表わすデータを出力する。
【００１２】
慣性航法部１２０は、角速度センサ１１１が計測した角速度ωｂや加速度センサ１１２が計測した加速度ａｂに基づいて、慣性座標系における移動体８００の姿勢角ｑｎや速度ｖｎを推定する。推定の詳細については、後述する。
慣性航法部１２０は、例えば、データを処理するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの処理装置が、プログラムを実行することにより実現してもよいし、ハードウェアにより実現してもよい。あるいは、ハードウェアとソフトウェアとの組み合わせにより実現してもよい。速度減算部１３０、第一推定部１４０、第二推定部１５０なども同様である。
【００１３】
速度減算部１３０は、ＧＰＳ受信機１１３が計測した速度ｖｇと、慣性航法部１２０が推定した速度ｖｎとの差ｄｖ＝ｖｇ−ｖｎを計算する。ここで、速度ｖｇ及び速度ｖｎは、ともに三次の縦ベクトルであるから、速度減算部１３０が計算する速度の差ｄｖも三次の縦ベクトルである。速度減算部１３０は、計算した速度の差ｄｖを表わすデータを出力する。
【００１４】
第一推定部１４０は、慣性航法部１２０が推定した姿勢角ｑｎや速度ｖｎと、速度減算部１３０が計算した速度の差ｄｖとに基づいて、角速度センサ１１１が計測した角速度ωｂのバイアス誤差ｂｇや慣性航法部１２０が推定した姿勢角ｑｎの誤差εを推定する。なお、第一推定部１４０が推定する角速度ωｂのバイアス誤差ｂｇは、例えば、角速度ωｂの三つの成分に対応して、移動体座標系のＸＹＺ三軸それぞれについてのバイアス誤差を成分とする三次の縦ベクトルである。また、第一推定部１４０が推定する姿勢角ｑｎの誤差εは、例えば、慣性座標系のｘｙｚ三軸それぞれを中心とする回転誤差を成分とする三次の縦ベクトルである。なお、推定の詳細については、後述する。第一推定部１４０は、推定した角速度ωｂのバイアス誤差ｂｇや姿勢角ｑｎの誤差εを表わすデータを出力する。
【００１５】
第二推定部１５０は、速度減算部１３０が計算した速度の差ｄｖと、ＧＰＳ受信機１１３が推定した精度とに基づいて、慣性航法部１２０が推定した速度ｖｎの誤差δｖを推定する。なお、第二推定部１５０が推定する速度ｖｎの誤差δｖは、例えば、慣性座標系のｘｙｚ三軸それぞれについての誤差を成分とする三次の縦ベクトルである。推定の詳細ついては、後述する。第二推定部１５０は、推定した速度の誤差δｖを表わすデータを出力する。
【００１６】
慣性航法部１２０は、第一推定部１４０や第二推定部１５０が推定した誤差に基づいて、誤差の補正を行い、誤差を補正した姿勢角ｑや速度ｖを算出する。慣性航法部１２０は、算出した慣性座標系における姿勢角ｑや速度ｖを表わすデータを出力する。
【００１７】
姿勢角出力部１９１は、慣性航法部１２０が出力した姿勢角ｑを表わすデータを入力する。姿勢角出力部１９１は、入力したデータが表わす姿勢角ｑを、例えば、表示装置を用いて表示したり、送信装置を用いて送信したりするなどして、外部に出力する。
【００１８】
速度出力部１９２は、慣性航法部１２０が出力した速度ｖを表わすデータを入力する。速度出力部１９２は、姿勢角出力部１９１と同様、入力したデータが表わす速度ｖを、外部に出力する。
【００１９】
位置出力部１９３は、ＧＰＳ受信機１１３が出力した位置ｐｇを表わすデータを入力する。位置出力部１９３は、姿勢角出力部１９１と同様、入力したデータが表わす位置ｐｇを、外部に出力する。
【００２０】
図２は、この実施の形態における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図である。
慣性航法部１２０は、角速度誤差補正部１２１、姿勢角外挿計算部１２２、姿勢角誤差補正部１２３、姿勢角記憶部１２４、方向余弦行列計算部１２５、速度増分計算部１２６、速度外挿計算部１２７、速度誤差補正部１２８、速度記憶部１２９を有する。
【００２１】
角速度誤差補正部１２１は、角速度センサ１１１が出力した角速度ωｂを表わすデータと、第一推定部１４０が出力した角速度ωｂのバイアス誤差ｂｇを表わすデータとを入力する。角速度誤差補正部１２１は、入力したデータが表わす角速度ωｂと、バイアス誤差ｂｇとに基づいて、角速度ωｂの誤差を補正して、誤差を補正した角速度ωｂ＋を算出する。角速度誤差補正部１２１が算出する角速度ωｂ＋は、角速度ωｂと同様、移動体座標系におけるＸＹＺ三軸それぞれを中心とする角速度を成分とする三次の縦ベクトルである。角速度誤差補正部１２１は、算出した角速度ωｂ＋を表わすデータを出力する。
角速度誤差補正部１２１は、例えば、次の計算式を用いて、角速度ωｂ＋を算出する。
【数１１】
ここで、ωｂ＋ｋは、角速度誤差補正部１２１が算出する時刻ｋにおける角速度を表わす三次の縦ベクトル。ωｂｋは、角速度センサ１１１が計測した時刻ｋにおける角速度を表わす三次の縦ベクトル。ｂｇは、第一推定部１４０が推定した角速度ωｂのバイアス誤差を表わす三次の縦ベクトル。
【００２２】
姿勢角記憶部１２４は、慣性座標系における移動体８００の姿勢角ｑを表わすデータを記憶している。姿勢角記憶部１２４は、例えば、姿勢角ｑを表わすデータとして、コータニオン（ｑｕａｔｅｒｎｉｏｎ）表示形式による四つの実数ｑ０，ｑ１，ｑ２，ｑ３を表わすデータを記憶する。
姿勢角記憶部１２４は、あらかじめ、例えば、移動体８００が水平状態で静止しているときに、図示していないコンパスなどの方位センサが計測した移動体８００の向きに基づいて算出した姿勢角を用いて、記憶した姿勢角ｑを表わすデータを初期化しておく。
【００２３】
速度記憶部１２９は、慣性座標系における移動体８００の速度ｖを表わすデータを記憶している。速度記憶部１２９は、例えば、速度ｖを表わすデータとして、慣性座標系におけるｘｙｚ三軸それぞれの方向の速度を表わす三つの実数ｖｘ，ｖｙ，ｖｚを表わすデータを記憶する。
速度記憶部１２９は、あらかじめ、例えば、移動体８００は静止しているときに求めた移動体８００の速度（すなわち「０」）を用いて、記憶した速度ｖを表わすデータを初期化しておく。
【００２４】
姿勢角記憶部１２４および速度記憶部１２９が記憶した姿勢角ｑおよび速度ｖは、その後、計測のたびに更新される。以下、時刻ｔｋにおける計測により更新された姿勢角ｑおよび速度ｖを、それぞれ「ｑｋ」「ｖｋ」と表記する。
【００２５】
姿勢角外挿計算部１２２は、姿勢角記憶部１２４が記憶した慣性座標系における姿勢角ｑを表わすデータと、角速度誤差補正部１２１が出力した補正した移動体座標系における角速度ωｂ＋を表わすデータとを入力する。姿勢角外挿計算部１２２は、入力したデータが表わす姿勢角ｑと角速度ωｂ＋とに基づいて、外挿計算をし、慣性座標系における新しい姿勢角ｑｎを算出する。姿勢角外挿計算部１２２は、算出した姿勢角ｑｎを表わすデータを出力する。
姿勢角外挿計算部１２２は、例えば、次の計算式を用いて、新しい姿勢角ｑｎを算出する。
【数１２】
ここで、ｑｎｋは、姿勢角外挿計算部１２２が算出する時刻ｔｋにおける姿勢角のコータニオン表示を成分とする四次の縦ベクトル。ｑｋ−１は、姿勢角記憶部１２４が記憶した時刻ｔｋ−１における姿勢角のコータニオン表示を成分とする四次の縦ベクトル。Ｔｓは、角速度センサ１１１が角速度ωｂを計測した周期を表わすスカラー値であり、Ｔｓ＝ｔｋ−ｔｋ−１。Ωｂｋは、四行四列の正方行列。ωＸ，ωＹ，ωＺは、角速度誤差補正部１２１が補正した時刻ｔｋにおける角速度ωｂ＋ｋのＸＹＺ三軸それぞれを中心すとする成分。
【００２６】
姿勢角誤差補正部１２３は、姿勢角外挿計算部１２２が出力した姿勢角ｑｎを表わすデータと、第一推定部１４０が出力した姿勢角ｑｎの誤差εを表わすデータとを入力する。姿勢角誤差補正部１２３は、入力したデータが表わす姿勢角ｑｎと、誤差εとに基づいて、姿勢角ｑｎの誤差を補正して、補正した姿勢角ｑｎ＋を算出する。姿勢角誤差補正部１２３は、補正した姿勢角ｑｎ＋を表わすデータを出力する。
姿勢角誤差補正部１２３は、例えば、次の計算式を用いて、補正した姿勢角ｑｎ＋を算出する。
【数１３】
ここで、ｑｎ＋ｋは、姿勢角誤差補正部１２３が算出する時刻ｔｋにおける補正した姿勢角のコータニオン表示を成分とする四次の縦ベクトル。ｑｎｋは、姿勢角外挿計算部１２２が算出した時刻ｔｋにおける姿勢角のコータニオン表示を成分とする四次の縦ベクトル。○で囲まれた×は、コータニオンの乗算。δｑは、コータニオンを表わす四次の縦ベクトル。εは、第一推定部１４０が算出した姿勢角の誤差を表わす三次の縦ベクトル。‖ε‖は、ベクトルεのノルムであるスカラー値。
【００２７】
姿勢角記憶部１２４は、姿勢角誤差補正部１２３が出力した姿勢角ｑｎ＋を表わすデータを入力する。姿勢角記憶部１２４は、入力したデータを、姿勢角ｑを表わすデータとして記憶する。姿勢角記憶部１２４が記憶した姿勢角ｑを表わすデータは、姿勢角出力部１９１が入力して、外部に出力する。
【００２８】
方向余弦行列計算部１２５は、姿勢角誤差補正部１２３が出力した補正した姿勢角ｑｎ＋を表わすデータを入力する。方向余弦行列計算部１２５は、入力したデータが表わす姿勢角ｑｎ＋に基づいて、方向余弦行列Ｃｎｂを算出する。方向余弦行列Ｃｎｂは、移動体座標系から慣性座標系への座標変換を表わす行列である。方向余弦行列計算部１２５は、算出した方向余弦行列Ｃｎｂを表わすデータを出力する。方向余弦行列計算部１２５が出力した方向余弦行列Ｃｎｂを表わすデータは、第一推定部１４０が入力し、誤差の推定に使用する。
方向余弦行列計算部１２５は、例えば、次の計算式を用いて、方向余弦行列Ｃｎｂを算出する。
【数１４】
ここで、Ｃｎｂは、方向余弦行列計算部１２５が算出する時刻ｔｋにおける方向余弦行列である三行三列の正方行列。ｑ０，ｑ１，ｑ２，ｑ３は、姿勢角誤差補正部１２３が補正した時刻ｔｋにおける姿勢角ｑｎ＋のコータニオン表示における各成分。
【００２９】
速度増分計算部１２６は、加速度センサ１１２が出力した移動体座標系における加速度ａｂを表わすデータと、方向余弦行列計算部１２５が出力した方向余弦行列Ｃｎｂを表わすデータとを入力する。速度増分計算部１２６は、入力したデータが表わす加速度ａｂと方向余弦行列Ｃｎｂとに基づいて、移動体座標系における加速度ａｂを慣性座標系における加速度に変換し、変換した加速度に基づいて、慣性座標系における移動体８００の速度の増分Δｖｎを算出する。速度増分計算部１２６は、算出した速度の増分Δｖｎを表わすデータを出力する。速度増分計算部１２６が出力した速度の増分Δｖｎを表わすデータは、第一推定部１４０が入力し、誤差の推定に使用する。
速度増分計算部１２６は、例えば、次の計算式により、速度の増分Δｖｎを算出する。
【数１５】
ここで、Δｖｎｋは、速度増分計算部１２６が算出する時刻ｔｋ−１から時刻ｔｋまでの間の速度の増分を表わす三次の縦ベクトル。Ｔｓは、加速度センサ１１２が加速度を計測した周期を表わすスカラー値で、数１２におけるＴｓと同じく、Ｔｓ＝ｔｋ−ｔｋ−１。Ｃｎｂは、方向余弦行列計算部１２５が算出した方向余弦行列である三行三列の正方行列。ａｂｋは、加速度センサ１１２が計測した時刻ｔｋにおける加速度を表わす三次の縦ベクトル。ｇｎは、慣性座標系における重力加速度を表わす三次の縦ベクトル。
【００３０】
速度外挿計算部１２７は、速度記憶部１２９が記憶した慣性座標系における速度ｖを表わすデータと、速度増分計算部１２６が出力した慣性座標系における速度の増分Δｖｎを表わすデータとを入力する。速度外挿計算部１２７は、入力したデータが表わす速度ｖと速度の増分Δｖｎとに基づいて、外挿計算をし、慣性座標系における新しい速度ｖｎを算出する。速度外挿計算部１２７は、算出した速度ｖｎを表わすデータを出力する。速度外挿計算部１２７が出力した速度ｖｎを表わすデータは、速度減算部１３０が入力し、速度の差ｄｖの算出に使用する。
速度外挿計算部１２７は、例えば、次の計算式を用いて、速度ｖｎを算出する。
【数１６】
ここで、ｖｎｋは、速度外挿計算部１２７が算出する時刻ｔｋにおける速度を表わす三次の縦ベクトル。ｖｋ−１は、速度記憶部１２９が記憶した時刻ｔｋ−１における速度を表わす三次の縦ベクトル。Δｖｎｋは、速度増分計算部１２６が算出した時刻ｔｋ−１から時刻ｔｋまでの間の速度の増分を表わす三次の縦ベクトル。
【００３１】
速度誤差補正部１２８は、速度外挿計算部１２７が出力した慣性座標系における速度ｖｎを表わすデータと、第二推定部１５０が出力した速度の誤差δｖを表わすデータとを入力する。速度誤差補正部１２８は、入力したデータが表わす速度ｖｎと速度の誤差δｖとに基づいて、速度ｖｎの誤差を補正して、補正した速度ｖｎ＋を算出する。速度誤差補正部１２８は、算出した速度ｖｎ＋を表わすデータを出力する。
速度誤差補正部１２８は、例えば、次の計算式を用いて、速度ｖｎ＋を算出する。
【数１７】
ここで、ｖｎ＋ｋは、速度誤差補正部１２８が算出する時刻ｔｋにおける補正した速度を表わす三次の縦ベクトル。ｖｎｋは、速度外挿計算部１２７が算出した時刻ｔｋにおける速度を表わす三次の縦ベクトル。δｖｋは、第二推定部１５０が推定した時刻ｔｋにおける速度の誤差を表わす三次の縦ベクトル。
【００３２】
なお、ＧＰＳ受信機１１３が移動体８００の位置・速度を計測していない場合、速度誤差補正部１２８は、速度誤差の補正をせず、入力したデータが表わす速度ｖｎを、そのまま速度ｖｎ＋とする。
【００３３】
速度記憶部１２９は、速度誤差補正部１２８が出力した慣性座標系における速度ｖｎ＋を表わすデータを入力する。速度記憶部１２９は、入力したデータを、速度ｖを表わすデータとして記憶する。速度記憶部１２９が記憶した速度ｖを表わすデータは、速度出力部１９２が入力して、外部に出力する。
【００３４】
図３は、この実施の形態における第一推定部１４０の機能ブロックの構成の一例を示す詳細ブロック図である。
第一推定部１４０は、カルマンフィルタを用いて、角速度センサ１１１が計測する角速度ωｂのバイアス誤差ｂｇと、慣性航法部１２０が推定する姿勢角の誤差εとを推定する。
第一推定部１４０が使用するカルマンフィルタは、例えば、次の式で表わされる六次の縦ベクトルｘを状態量とし、三次の縦ベクトルΔｚを観測量とする。
【数１８】
【００３５】
第一推定部１４０は、観測行列計算部１４１、推移行列計算部１４２、誤差共分散外挿計算部１４３、カルマンゲイン計算部１４４、誤差共分散観測更新計算部１４５、誤差共分散記憶部１４６、状態量外挿計算部１４７、状態量観測更新計算部１４８、状態量記憶部１４９を有する。
【００３６】
観測行列計算部１４１は、慣性航法部１２０の速度増分計算部１２６が出力した速度の増分Δｖｎを表わすデータを入力する。観測行列計算部１４１は、入力したデータが表わす速度の増分Δｖｎに基づいて、カルマンフィルタで使用する観測行列Ｈを算出する。観測行列計算部１４１は、算出した観測行列Ｈを表わすデータを出力する。
観測行列計算部１４１は、例えば、次の計算式を用いて、観測行列Ｈを算出する。
【数１９】
ここで、Ｈは、観測行列計算部１４１が算出する観測行列である三行六列の行列。０３×３は、三行三列の零行列。Ωｖは、三行三列の正方行列。Δｖｎｘ，Δｖｎｙ，Δｖｎｚは、速度増分計算部１２６が算出した時刻ｔｋ−１から時刻ｔｋまでの間の速度の増分Δｖｎｋのｘｙｚ三軸それぞれの方向成分。
【００３７】
なお、観測行列計算部１４１が計算する観測行列Ｈは、ＧＰＳ受信機１１３が移動体８００の位置ｐｇや速度ｖｇを計測し、速度減算部１３０が速度の差ｄｖを算出した場合のみ使用される。このため、観測行列計算部１４１は、ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したか否かを判定し、計測したと判定した場合は観測行列Ｈを算出し、計測していないと判定した場合は観測行列Ｈを算出しない構成としてもよい。
【００３８】
推移行列計算部１４２は、慣性航法部１２０の方向余弦行列計算部１２５が出力した方向余弦行列Ｃｎｂを表わすデータを入力する。推移行列計算部１４２は、入力したデータが表わす方向余弦行列Ｃｎｂに基づいて、カルマンフィルタで使用する推移行列Φを算出する。推移行列計算部１４２は、算出した推移行列Φを表わすデータを出力する。
推移行列計算部１４２は、例えば、次の計算式を用いて、推移行列Φを算出する。
【数２０】
ここで、Φは、推移行列計算部１４２が算出する推移行列である六行六列の正方行列。Ｉ３×３は、三行三列の単位行列。０３×３は、三行三列の零行列。Ｔｓは、角速度センサ１１１及び加速度センサ１１２が角速度や加速度を計測する周期を表わすスカラー値で、数１２や数１５におけるＴｓと同じく、Ｔｓ＝ｔｋ−ｔｋ−１。
【００３９】
誤差共分散記憶部１４６は、カルマンフィルタにおける誤差共分散行列Ｐを表わすデータを記憶している。誤差共分散記憶部１４６は、例えば、六行六列の行列である誤差共分散行列Ｐを表わすデータとして、６×６＝３６個の実数を表わすデータを記憶する。誤差共分散記憶部１４６は、例えば、あらかじめ、誤差共分散行列Ｐの初期値として設定した所定の行列を用いて、記憶した誤差共分散行列Ｐを表わすデータを初期化しておく。
【００４０】
状態量記憶部１４９は、カルマンフィルタにおける状態量ｘを表わすデータを記憶している。状態量記憶部１４９は、例えば、六次の縦ベクトルである状態量ｘを表わすデータとして、六個の実数を表わすデータを記憶する。状態量記憶部１４９は、例えば、あらかじめ、状態量ｘの初期値として設定した所定のベクトルを用いて、記憶した状態量ｘを表わすデータを初期化しておく。
【００４１】
誤差共分散記憶部１４６および状態量記憶部１４９が記憶した誤差共分散行列Ｐおよび状態量ｘは、その後、計測のたびに更新される。以下、時刻ｔｋにおける計測により更新された誤差共分散行列Ｐおよび状態量ｘを、それぞれ「Ｐｋ」「ｘｋ」と表記する。
【００４２】
誤差共分散外挿計算部１４３は、推移行列計算部１４２が出力した推移行列Φを表わすデータと、誤差共分散記憶部１４６が記憶した誤差共分散行列Ｐを表わすデータとを入力する。誤差共分散外挿計算部１４３は、入力したデータが表わす推移行列Φと、誤差共分散行列Ｐとに基づいて、誤差共分散行列の外挿計算をして、外挿した誤差共分散行列Ｐ−を算出する。誤差共分散外挿計算部１４３は、算出した誤差共分散行列Ｐ−を表わすデータを出力する。
誤差共分散外挿計算部１４３は、例えば、次の計算式を用いて、誤差共分散行列Ｐ−を算出する。
【数２１】
ここで、Ｐ−ｋは、誤差共分散外挿計算部１４３が算出する時刻ｔｋにおける外挿した誤差共分散行列である六行六列の正方行列。Ｐｋ−１は、誤差共分散記憶部１４６が記憶した時刻ｔｋ−１における誤差共分散行列である六行六列の正方行列。Φは、推移行列計算部１４２が算出した推移行列である六行六列の正方行列。ΦＴは、行列Φの転置行列である六行六列の行列。Ｑは、あらかじめ設定されたプロセスノイズ行列である六行六列の正方行列。
誤差共分散外挿計算部１４３は、プロセスノイズ行列Ｑとして、あらかじめ、設定された値を記憶している。
【００４３】
状態量外挿計算部１４７は、推移行列計算部１４２が出力した推移行列Φを表わすデータと、状態量記憶部１４９が記憶した状態量ｘを表わすデータとを入力する。状態量外挿計算部１４７は、入力したデータが表わす推移行列Φと状態量ｘとに基づいて、状態量の外挿計算をして、外挿した状態量ｘ−を算出する。状態量外挿計算部１４７は、算出した外挿した状態量ｘ−を表わすデータを出力する。
状態量外挿計算部１４７は、例えば、次の計算式を用いて、外挿した状態量ｘ−を算出する。
【数２２】
ここで、ｘ−ｋは、状態量外挿計算部１４７が算出する時刻ｔｋにおける外挿した状態量である六次の縦ベクトル。Φは、推移行列計算部１４２が算出した推移行列である六行六列の正方行列。ｘｋ−１は、状態量記憶部１４９が記憶した時刻ｔｋ−１における状態量である六次の縦ベクトル。
【００４４】
カルマンゲイン計算部１４４は、観測行列計算部１４１が出力した観測行列Ｈを表わすデータと、誤差共分散外挿計算部１４３が出力した誤差共分散行列Ｐ−を表わすデータとを入力する。カルマンゲイン計算部１４４は、入力したデータが表わす観測行列Ｈと誤差共分散行列Ｐ−とに基づいて、カルマンフィルタにおけるカルマンゲインＫを算出する。カルマンゲイン計算部１４４は、算出したカルマンゲインＫを表わすデータを出力する。
カルマンゲイン計算部１４４は、例えば、次の計算式を用いて、カルマンゲインＫを算出する。
【数２３】
ここで、Ｋは、カルマンゲイン計算部１４４が算出する時刻ｔｋにおけるカルマンゲインである六行三列の行列。Ｐ−ｋは、誤差共分散外挿計算部１４３が算出した時刻ｔｋにおける外挿した誤差共分散行列である六行六列の正方行列。Ｈは、観測行列計算部１４１が算出した時刻ｔｋにおける観測行列である三行六列の行列。ＨＴは、観測行列Ｈの転置行列である六行三列の行列。Ｒは、あらかじめ設定された観測誤差の共分散行列である三行三列の行列。（〜）−１は、逆行列。
カルマンゲイン計算部１４４は、観測誤差の共分散行列Ｒとして、あらかじめ、設定された値を記憶している。
【００４５】
なお、カルマンゲイン計算部１４４が計算するカルマンゲインＫは、ＧＰＳ受信機１１３が移動体８００の位置ｐｇや速度ｖｇを計測し、速度減算部１３０が速度の差ｄｖを算出した場合のみ使用される。このため、カルマンゲイン計算部１４４は、ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したか否かを判定し、計測したと判定した場合はカルマンゲインＫを算出し、計測していないと判定した場合はカルマンゲインＫを算出しない構成としてもよい。
【００４６】
誤差共分散観測更新計算部１４５は、観測行列計算部１４１が出力した観測行列Ｈを表わすデータと、誤差共分散外挿計算部１４３が出力した外挿した誤差共分散行列Ｐ−を表わすデータと、カルマンゲイン計算部１４４が出力したカルマンゲインＫを表わすデータとを入力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したと判定した場合、誤差共分散観測更新計算部１４５は、入力したデータが表わす観測行列Ｈと外挿した誤差共分散行列Ｐ−とカルマンゲインＫとに基づいて、誤差共分散行列を観測更新し、観測更新した誤差共分散行列Ｐ＋を算出する。誤差共分散観測更新計算部１４５は、算出した観測更新した誤差共分散行列Ｐ＋を表わすデータを出力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測していないと判定した場合、誤差共分散観測更新計算部１４５は、入力したデータが表わす外挿した誤差共分散行列Ｐ−をそのまま誤差共分散行列Ｐ＋とし、誤差共分散行列Ｐ＋を表わすデータを出力する。
誤差共分散観測更新計算部１４５は、例えば、次の計算式を用いて、観測更新した誤差共分散行列Ｐ＋を算出する。
【数２４】
ここで、Ｐ＋ｋは、誤差共分散観測更新計算部１４５が算出する時刻ｔｋにおける観測更新した誤差共分散行列である六行六列の行列。Ｉ６×６は、六行六列の単位行列。Ｋは、カルマンゲイン計算部１４４が算出したカルマンゲインである六行三列の行列。Ｈは、観測行列計算部１４１が算出した観測行列である三行六列の行列。Ｐ−ｋは、誤差共分散外挿計算部１４３が算出した外挿した誤差共分散行列である六行六列の行列。
【００４７】
誤差共分散記憶部１４６は、誤差共分散観測更新計算部１４５が出力した誤差共分散行列Ｐ＋を表わすデータを入力する。誤差共分散記憶部１４６は、入力したデータを、誤差共分散行列Ｐを表わすデータとして記憶する。
【００４８】
状態量観測更新計算部１４８は、状態量外挿計算部１４７が出力した外挿した状態量ｘ−を表わすデータと、カルマンゲイン計算部１４４が出力したカルマンゲインＫを表わすデータと、速度減算部１３０が出力した速度の差ｄｖを表わすデータとを入力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したと判定した場合、状態量観測更新計算部１４８は、入力したデータが表わす外挿した状態量ｘ−とカルマンゲインＫと速度の差ｄｖとに基づいて、状態量を観測更新し、観測更新した状態量ｘ＋を算出する。状態量観測更新計算部１４８は、算出した観測更新した状態量ｘ＋を表わすデータを出力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測していないと判定した場合、状態量観測更新計算部１４８は、入力したデータが表わす外挿した状態量ｘ−をそのまま状態量ｘ＋とし、状態量ｘ＋を表わすデータを出力する。
状態量観測更新計算部１４８は、例えば、次の計算式を用いて、観測更新した状態量ｘ＋を算出する。
【数２５】
ここで、ｘ＋ｋは、状態量観測更新計算部１４８が算出する観測更新した状態量である六次の縦ベクトル。ｘ−ｋは、状態量外挿計算部１４７が算出した外挿した状態量である六次の縦ベクトル。Ｋは、カルマンゲイン計算部１４４が算出したカルマンゲインである六行三列の行列。Δｚは、観測量である三次の縦ベクトル。ｄｖは、速度減算部１３０が算出した速度の差である三次の縦ベクトル。
【００４９】
状態量記憶部１４９は、状態量観測更新計算部１４８が出力した状態量ｘ＋を表わすデータを入力する。状態量記憶部１４９は、入力したデータを、状態量ｘを表わすデータとして記憶する。状態量記憶部１４９が記憶した状態量ｘ（角速度ωｂのバイアス誤差ｂｇおよび姿勢角ｑの誤差ε）を表わすデータは、慣性航法部１２０が入力し、誤差の補正に使用する。
【００５０】
図４は、この実施の形態における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図である。
第二推定部１５０は、ＧＰＳ受信機１１３が推定したＧＰＳ受信機１１３の計測精度に基づいて、慣性航法部１２０が推定した速度ｖの誤差δｖを推定する。
第二推定部１５０は、ゲイン算出部１５１、速度誤差推定部１５２を有する。
【００５１】
ゲイン算出部１５１は、ＧＰＳ受信機１１３が出力した計測の精度を表わすデータを入力する。ゲイン算出部１５１は、入力したデータが表わす精度に基づいて、速度誤差のゲインＫｖを算出する。ゲイン算出部１５１は、算出したゲインＫｖを表わすデータを出力する。
ゲイン算出部１５１が算出するゲインＫｖは、例えば、０以上１以下の値をとるスカラー値である。この場合、例えば、ＧＰＳ受信機１１３の計測精度が高く、ＧＰＳ受信機１１３が計測した速度ｖｇが完全に信頼できる場合、ゲイン算出部１５１は、ゲインＫｖとして「１」を算出する。逆に、ＧＰＳ受信機１１３の計測精度が低く、ＧＰＳ受信機１１３が計測した速度ｖｇが全く信頼できない場合、ゲイン算出部１５１は、ゲインＫｖとして「０」を算出する。通常は、ＧＰＳ受信機１１３が計測した速度ｖｇが完全に信頼できることも全く信頼できないこともないので、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した精度に基づいて、ゲインＫＶとして「０」と「１」との値を算出する。ゲイン算出部１５１は、ＧＰＳ受信機１１３の計測精度が高いほど、算出するゲインＫｖの値を大きくする。
ゲイン算出部１５１は、例えば、あらかじめ、ＧＰＳ受信機１１３の計測精度をゲインＫｖに変換する変換テーブルを表わすデータを記憶しておく。ゲイン算出部１５１は、記憶したデータが表わす変換テーブルを参照し、入力したデータが表わす計測精度に対応するゲインＫｖを算出する。
【００５２】
なお、ゲイン算出部１５１が算出するゲインＫｖは、例えば、三行三列の行列であってもよい。例えば、ＧＰＳ受信機１１３が、水平方向の精度と垂直方向の精度というように、方向によって異なる複数の精度を算出する場合、ゲイン算出部１５１は、慣性座標系におけるｘｙｚ三軸の各軸方向によって異なるゲインを算出して、行列形式で表わされるゲインＫｖとする構成であってもよい。
【００５３】
また、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した計測の精度にかかわらず、あらかじめ設定した一定の値（例えば「０．５」）をゲインとして算出する構成であってもよい。その場合、ＧＰＳ受信機１１３は、計測の精度を推定する機能のないものであってもよい。
【００５４】
速度誤差推定部１５２は、速度減算部１３０が出力した速度の差ｄｖを表わすデータと、ゲイン算出部１５１が出力したゲインＫｖを表わすデータとを入力する。速度誤差推定部１５２は、入力したデータが表わす速度の差ｄｖとゲインＫｖとに基づいて、慣性航法部１２０が推定した速度の誤差δｖを算出する。速度誤差推定部１５２は、算出した速度の誤差δｖを表わすデータを出力する。速度誤差推定部１５２が出力した速度の誤差δｖを表わすデータは、慣性航法部１２０が入力し、誤差の補正に使用する。
速度誤差推定部１５２は、例えば、次の計算式を用いて、速度の誤差δｖを算出する。
【数２６】
ここで、δｖは、速度誤差推定部１５２が推定する速度の誤差を表わす三次の縦ベクトル。Ｋｖは、ゲイン算出部１５１が算出した速度誤差のゲインを表わすスカラー値もしくは三行三列の行列。ｄｖは、速度減算部１３０が算出した速度の差を表わす三次の縦ベクトル。
【００５５】
次に、動作について説明する。
【００５６】
図５は、この実施の形態における航法装置１００が移動体８００の速度・姿勢角を推定する航法処理の全体的な流れの一例を示すフローチャート図である。
航法処理は、初期化処理Ｓ５１０、外挿計算処理Ｓ５２０、ＧＰＳ計測判定工程Ｓ５４０、観測更新処理Ｓ５５０を有する。
【００５７】
初期化処理Ｓ５１０において、航法装置１００は、記憶したデータを初期化する。
外挿計算処理Ｓ５２０において、航法装置１００は、角速度センサ１１１や加速度センサ１１２が計測した角速度・加速度に基づいて、推定した速度・姿勢角などを外挿計算する。
【００５８】
ＧＰＳ計測判定工程Ｓ５４０において、航法装置１００は、ＧＰＳ受信機１１３が移動体８００の位置・速度を計測したか否かを判定する。
ＧＰＳ受信機１１３が移動体８００の位置・速度を計測したと判定した場合、航法装置１００は、観測更新処理Ｓ５５０へ進む。
ＧＰＳ受信機１１３が移動体８００の位置・速度を計測していないと判定した場合、航法装置１００は、外挿計算処理Ｓ５２０に戻る。
【００５９】
観測更新処理Ｓ５５０において、航法装置１００は、ＧＰＳ受信機１１３が計測した移動体８００の速度に基づいて、推定した速度・姿勢角などを観測更新する。その後、航法装置１００は、外挿計算処理Ｓ５２０に戻る。
【００６０】
図６は、この実施の形態における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図である。
初期化処理Ｓ５１０は、姿勢角初期化工程Ｓ５１１、速度初期化工程Ｓ５１２、誤差共分散初期化工程Ｓ５１４、状態量初期化工程Ｓ５１５を有する。
【００６１】
姿勢角初期化工程Ｓ５１１において、姿勢角記憶部１２４は、記憶した姿勢角ｑを初期化する。
速度初期化工程Ｓ５１２において、速度記憶部１２９は、記憶した速度ｖを初期化する。
誤差共分散初期化工程Ｓ５１４において、誤差共分散記憶部１４６は、記憶した誤差共分散行列Ｐを初期化する。
状態量初期化工程Ｓ５１５において、状態量記憶部１４９は、記憶した状態量ｘを初期化する。
【００６２】
図７は、この実施の形態における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図である。
外挿計算処理Ｓ５２０は、角速度誤差補正工程Ｓ５２１、姿勢角外挿計算工程Ｓ５２２、姿勢角誤差補正工程Ｓ５２３、方向余弦行列計算工程Ｓ５２４、速度増分計算工程Ｓ５２５、速度外挿計算工程Ｓ５２６、推移行列計算工程Ｓ５３１、誤差共分散外挿計算工程Ｓ５３２、状態量外挿計算工程Ｓ５３３を有する。
【００６３】
角速度誤差補正工程Ｓ５２１において、角速度誤差補正部１２１は、状態量記憶部１４９が記憶した状態量ｘのうち角速度のバイアス誤差ｂｇに基づいて、角速度センサ１１１が計測した角速度ωｂを補正し、補正した角速度ωｂ＋を算出する。
姿勢角外挿計算工程Ｓ５２２において、姿勢角外挿計算部１２２は、角速度誤差補正工程Ｓ５２１で角速度誤差補正部１２１が補正した角速度ωｂ＋に基づいて、姿勢角記憶部１２４が記憶した姿勢角ｑを外挿し、外挿した姿勢角ｑｎを算出する。
姿勢角誤差補正工程Ｓ５２３において、姿勢角誤差補正部１２３は、状態量記憶部１４９が記憶した状態量ｘのうち姿勢角の誤差εに基づいて、姿勢角外挿計算工程Ｓ５２２で姿勢角外挿計算部１２２が外挿した姿勢角ｑｎを補正し、補正した姿勢角ｑｎ＋を算出する。姿勢角記憶部１２４は、姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋を、新しい姿勢角ｑとして記憶する。
方向余弦行列計算工程Ｓ５２４において、方向余弦行列計算部１２５は、姿勢角誤差補正工程Ｓ５２３で姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋に基づいて、方向余弦行列Ｃｎｂを算出する。
速度増分計算工程Ｓ５２５において、速度増分計算部１２６は、方向余弦行列計算工程Ｓ５２４で方向余弦行列計算部１２５が算出した方向余弦行列Ｃｎｂに基づいて、加速度センサ１１２が計測した移動体座標系における加速度ａｂを、慣性座標系における加速度に座標変換し、座標変換した慣性座標系における加速度に基づいて、速度の増分Δｖｎを算出する。
速度外挿計算工程Ｓ５２６において、速度外挿計算部１２７は、速度増分計算工程Ｓ５２５で速度増分計算部１２６が算出した速度の増分Δｖｎに基づいて、速度記憶部１２９が記憶した速度ｖを外挿し、外挿した速度ｖｎを算出する。速度記憶部１２９は、速度外挿計算部１２７が外挿した速度ｖｎを、暫定的な速度ｖとして記憶する。
【００６４】
推移行列計算工程Ｓ５３１において、推移行列計算部１４２は、方向余弦行列計算工程Ｓ５２４で方向余弦行列計算部１２５が算出した方向余弦行列Ｃｎｂに基づいて、推移行列Φを算出する。
誤差共分散外挿計算工程Ｓ５３２において、誤差共分散外挿計算部１４３は、推移行列計算工程Ｓ５３１で推移行列計算部１４２が算出した推移行列Φに基づいて、誤差共分散記憶部１４６が記憶した誤差共分散行列Ｐを外挿し、外挿した誤差共分散行列Ｐ−を算出する。ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、誤差共分散外挿計算部１４３が算出した誤差共分散行列Ｐ−が、そのまま誤差共分散記憶部１４６が記憶する誤差共分散行列Ｐとなるので、誤差共分散記憶部１４６は、誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−を、暫定的に、誤差共分散行列Ｐとして記憶する。
状態量外挿計算工程Ｓ５３３において、状態量外挿計算部１４７は、推移行列計算工程Ｓ５３１で推移行列計算部１４２が算出した推移行列Φに基づいて、状態量記憶部１４９が記憶した状態量ｘを外挿し、外挿した状態量ｘ−を算出する。ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、状態量外挿計算部１４７が算出した外挿した状態量ｘ−が、そのまま状態量記憶部１４９が記憶する状態量ｘとなるので、状態量記憶部１４９は、状態量外挿計算部１４７が外挿した状態量ｘ−を、暫定的に、状態量ｘとして記憶する。
【００６５】
図８は、この実施の形態における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図である。
観測更新処理Ｓ５５０は、速度減算工程Ｓ５５１、観測行列計算工程Ｓ５５２、カルマンゲイン計算工程Ｓ５５３、誤差共分散観測更新計算工程Ｓ５５４、状態量観測更新計算工程Ｓ５５５、ゲイン算出工程Ｓ５６１、速度誤差推定工程Ｓ５６２、速度誤差補正工程Ｓ５６３を有する。
【００６６】
速度減算工程Ｓ５５１において、速度減算部１３０は、ＧＰＳ受信機１１３が計測した速度ｖｇと、速度外挿計算工程Ｓ５２６で速度外挿計算部１２７が外挿した速度ｖｎとに基づいて、速度の差ｄｖを算出する。
観測行列計算工程Ｓ５５２において、観測行列計算部１４１は、速度増分計算工程Ｓ５２５で速度増分計算部１２６が算出した速度の増分Δｖｎに基づいて、観測行列Ｈを算出する。
カルマンゲイン計算工程Ｓ５５３において、カルマンゲイン計算部１４４は、誤差共分散外挿計算工程Ｓ５３２で誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−と、観測行列計算工程Ｓ５５２で観測行列計算部１４１が算出した観測行列Ｈとに基づいて、カルマンゲインＫを算出する。
誤差共分散観測更新計算工程Ｓ５５４において、誤差共分散観測更新計算部１４５は、観測行列計算工程Ｓ５５２で観測行列計算部１４１が算出した観測行列Ｈと、カルマンゲイン計算工程Ｓ５５３でカルマンゲイン計算部１４４が計算したカルマンゲインＫとに基づいて、誤差共分散外挿計算工程Ｓ５３２で誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−を観測更新し、観測更新した誤差共分散行列Ｐ＋を算出する。誤差共分散記憶部１４６は、誤差共分散観測更新計算部１４５が観測更新した誤差共分散行列Ｐ＋を、誤差共分散行列Ｐとして記憶する。
状態量観測更新計算工程Ｓ５５５において、状態量観測更新計算部１４８は、カルマンゲイン計算工程Ｓ５５３でカルマンゲイン計算部１４４が計算したカルマンゲインＫに基づいて、状態量外挿計算工程Ｓ５３３で状態量外挿計算部１４７が外挿した状態量ｘ−を観測更新し、観測更新した状態量ｘ＋を算出する。状態量記憶部１４９は、状態量観測更新計算部１４８が観測更新した状態量ｘ＋を、状態量ｘとして記憶する。
なお、ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、観測更新処理Ｓ５５０を実行しないので、誤差共分散記憶部１４６は、誤差共分散外挿計算工程Ｓ５３２で暫定的に記憶した誤差共分散行列Ｐ（すなわち、誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−）をそのまま記憶し続け、状態量記憶部１４９は、状態量外挿計算工程Ｓ５３３で暫定的に記憶した状態量ｘ（すなわち、状態量外挿計算部１４７が外挿した状態量ｘ−）をそのまま記憶し続けることとなる。
【００６７】
ゲイン算出工程Ｓ５６１において、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した計測の精度に基づいて、速度誤差のゲインＫｖを算出する。
速度誤差推定工程Ｓ５６２において、速度誤差推定部１５２は、速度減算工程Ｓ５５１で速度減算部１３０が算出した速度の差ｄｖと、ゲイン算出工程Ｓ５６１でゲイン算出部１５１が算出した速度誤差のゲインＫｖとに基づいて、速度の誤差δｖを算出する。
速度誤差補正工程Ｓ５６３において、速度誤差補正部１２８は、速度誤差推定工程Ｓ５６２で速度誤差推定部１５２が算出した速度の誤差δｖに基づいて、速度外挿計算工程Ｓ５２６で速度外挿計算部１２７が外挿した速度ｖｎを補正し、補正した速度ｖｎ＋を算出する。速度記憶部１２９は、速度誤差補正部１２８が補正した速度ｖｎ＋を、速度ｖとして記憶する。
【００６８】
慣性航法部１２０が推定した姿勢角ｑが誤差εを含んでいる場合、その誤差εは、方向余弦行列Ｃｎｂを介して、速度増分計算部１２６が座標変換した加速度に伝播し、速度増分計算部１２６が算出する速度の増分Δｖｎに反映する。誤差εが反映された速度の増分Δｖｎに基づいて観測行列計算部１４１が算出した観測行列Ｈを用いて、誤差εと、観測量Δｚ（速度の差ｄｖ）とを結びつけることにより、カルマンフィルタを用いて、状態量ｘ（バイアス誤差ｂｇ及び誤差ε）を推定することができる。
【００６９】
ここで、加速度センサ１１２が計測する加速度ａｂの誤差が大きい場合には、その誤差も、速度増分計算部１２６が算出する速度の増分Δｖｎに反映する。そのため、例えば、位置の誤差、速度の誤差、姿勢角の誤差、角速度センサの誤差、加速度センサの誤差を状態量とし、角速度センサや加速度センサに基づいて推定した位置及び速度と、ＧＰＳ受信機が計測した位置及び速度との差を観測量とする必要が生じる。
これに対し、加速度センサ１１２が計測する加速度ａｂの誤差が無視できるほど小さい場合には、バイアス誤差ｂｇと誤差εとのみを状態量としても、精度よく推定をすることができる。
【００７０】
航法装置１００の製造コストを低く抑えるため、角速度センサ１１１や加速度センサ１１２として、安価なセンサを使用する場合、角速度センサは、バイアス誤差が比較的大きいが、加速度センサには、バイアス誤差が比較的小さく、適切な補償をすることにより、バイアス誤差を無視できるものが存在する。
【００７１】
カルマンフィルタを用いた推定において、状態量ベクトルの次数は、推定に必要な計算量に大きな影響を与える。いま、状態量ベクトルの次数をｎ、プロセスノイズ行列の次数をｍ、観測量ベクトルの次数をｒとすると、カルマンフィルタによる１回の観測更新における乗算の回数は、以下の式で与えられる。
【数２７】
なお、加算の計算負荷は、乗算の計算負荷に比べてはるかに小さいので、推定に必要な計算量は、乗算の回数により見積もることができる。
【００７２】
この実施の形態における航法装置１００において、ｎ＝ｍ＝６、ｒ＝３であるから、Ｓｍ＝７８６である。
比較例として、位置の誤差（三次）、速度の誤差（三次）、姿勢角の誤差ε（三次）、角速度センサの誤差ｂｇ（三次）、加速度センサの誤差ｂａ（三次）を状態量（１５次）とし、位置の差（三次）、速度の差（三次）を観測量（六次）とした場合を考えると、ｎ＝ｍ＝１５、ｒ＝６であるから、Ｓｍ＝９８５５である。
このように、この実施の形態における航法装置１００は、比較例に対して、乗算の回数を１０分の１以下に抑えることができる。
【００７３】
ＧＰＳ受信機１１３が移動体８００の位置や速度を計測する頻度は、１秒間に１〜４回程度であるから、実時間で航法処理をするには、１秒間に１〜４回の観測更新処理をする必要がある。
【００７４】
この実施の形態における航法装置１００は、１回の処理に必要な計算量が少ないので、比較的処理能力の低いマイクロコンピュータなどを使用して、慣性航法部１２０や第一推定部１４０などを実現しても、実時間で処理を行うことができる。
このため、航法装置１００の製造コストを抑えることができる。
【００７５】
この実施の形態における航法装置１００の構成により、推定の精度が劣化しないことを確認するため、以下のような実験を行った。
【００７６】
（１）シミュレーションによる実験
実機での実験に先立ち、以下の条件のもと、シミュレーションによる模擬実験を行った。
角速度センサ１１１の誤差モデル：ノイズ誤差０．０５４度／秒／√Ｈｚ、バイアス誤差０．１９度／秒／√ｈ、スケールファクタ１．０％、初期化誤差０．１％。
加速度センサ１１２の誤差モデル：ノイズ誤差４１．７μｇ／√Ｈｚ、バイアス誤差０．０２８ｍ／ｓ２／√ｈ、スケールファクタ１．０％、初期化誤差０．１％。
ＧＰＳデータ：２００７年７月２０日のデータ。平均利用可能衛星数７。
【００７７】
比較対象として、位置誤差、速度誤差、姿勢角誤差、角速度バイアス誤差、加速度バイアス誤差を状態量（１５次）とする従来の疎結合ＧＰＳ／ＩＮＳ（比較例１）、更に、状態量にクロックバイアス誤差とクロックドリフト誤差を加えた（１７次）従来の密結合ＧＰＳ／ＩＮＳ（比較例２）についても、同じ条件でシミュレーションを行った。
【００７８】
その結果、この実施の形態における航法装置１００における姿勢角の推定誤差の二乗平均平方根（ＲＭＳ）は、ロール方向で０．３１度、ピッチ方向で０．４２度、ヨー方向で１．５５度であった。
これに対し、比較例１は、ロール方向で０．３３度、ピッチ方向で０．４１度、ヨー方向で１．３４度であった。比較例２は、ロール方向で０．２８度、ピッチ方向で０．３７度、ヨー方向で１．２２度であった。
【００７９】
この模擬実験の結果によれば、この実施の形態における航法装置１００の構成による推定の精度は、従来の構成とほとんど変わらない。
【００８０】
（２）実機による実験
模擬実験により、一応の確認が取れたので、更に、以下の条件のもと、実機による実験を行った。
センサ：自動車用低価格ＭＥＭＳ慣性センサ。計測周波数６４Ｈｚ。
ＧＰＳ受信機：計測周波数４Ｈｚ。
参照データの計測：リングレーザージャイロを用いたＧＰＳ／ＩＮＳ（姿勢角の推定誤差の二乗平均平方根０．１度）。
比較対象：模擬実験と同じ。
【００８１】
実験の結果、この実施の形態における航法装置１００における姿勢角の推定誤差の二乗平均平方根は、ロール方向で０．２４度、ピッチ方向で０．１７度、ヨー方向で１．４１度であった。
これに対し、比較例１は、ロール方向で０．２５度、ピッチ方向で０．２０度、ヨー方向で１．７８度であった。比較例２は、ロール方向で０．２４度、ピッチ方向で０．２５度、ヨー方向で１．３１度であった。
【００８２】
この実機による実験の結果によれば、この実施の形態における航法装置１００の構成による推定の精度は、従来の構成とほとんど変わらない。
【００８３】
以上のように、この実施の形態における航法装置１００によれば、推定の精度を劣化させることなく、カルマンフィルタの計算量を減らすことができる。
【００８４】
この実施の形態における航法装置１００は、移動体８００に搭載される航法装置である。
上記航法装置１００は、角速度計測部（角速度センサ１１１）と、加速度計測部（加速度センサ１１２）と、速度計測部（ＧＰＳ受信機１１３）と、姿勢角外挿計算部１２２と、速度増分計算部１２６と、速度外挿計算部１２７と、速度減算部１３０と、第一推定部１４０とを有する。
上記角速度計測部（角速度センサ１１１）は、上記移動体８００を基準とした第一座標系（移動体座標系）における上記移動体８００の角速度ωｂを計測する。
上記加速度計測部（加速度センサ１１２）は、上記第一座標系における上記移動体８００に加わる加速度ａｂを計測する。
上記速度計測部（ＧＰＳ受信機１１３）は、上記第一座標系と異なる第二座標系（慣性座標系）における上記移動体８００の速度ｖｇを計測する。
上記姿勢角外挿計算部１２２は、上記角速度計測部（角速度センサ１１１）が計測した角速度ωｂに基づいて、上記第二座標系における上記移動体８００の姿勢角ｑｎを計算する。
上記速度増分計算部１２６は、上記加速度計測部（加速度センサ１１２）が計測した加速度ａｂと、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎとに基づいて、上記第二座標系における上記移動体８００の速度の増分Δｖｎを計算する。
上記速度外挿計算部１２７は、上記速度増分計算部１２６が計算した速度の増分Δｖｎに基づいて、上記第二座標系における上記移動体８００の速度ｖｎを計算する。
上記速度減算部１３０は、上記速度外挿計算部１２７が計算した速度ｖｎと、上記速度計測部（ＧＰＳ受信機１１３）が計測した速度ｖｇとの差ｄｖを計算する。
上記第一推定部１４０は、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、上記速度増分計算部１２６が計算した速度の増分Δｖｎと、上記速度減算部１３０が計算した速度の差ｄｖとに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを推定する。
【００８５】
この実施の形態における航法装置１００によれば、姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、速度増分計算部１２６が計算した速度の増分Δｖｎと、速度減算部１３０が計算した速度の差ｄｖとに基づいて、第一推定部１４０が角速度の誤差ｂｇと、姿勢角の誤差εとを推定するので、推定にかかる計算量が少なく、航法装置１００の製造コストを低く抑えることができる。
【００８６】
この実施の形態における航法装置１００は、更に、角速度誤差補正部１２１と、姿勢角誤差補正部１２３とを有する。
上記角速度誤差補正部１２１は、上記第一推定部１４０が推定した角速度の誤差ｂｇに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度ωｂを補正する。
上記姿勢角外挿計算部１２２は、上記角速度誤差補正部１２１が補正した角速度ωｂ＋に基づいて、上記第二座標系における上記移動体８００の姿勢角ｑｎを計算する。
上記姿勢角誤差補正部１２３は、上記第一推定部１４０が推定した姿勢角の誤差εに基づいて、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎを補正する。
上記速度増分計算部１２６は、上記加速度計測部（加速度センサ１１２）が計測した加速度ａｂと、上記姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋とに基づいて、上記第二座標系における上記移動体８００の速度の増分Δｖｎを計算する。
上記第一推定部１４０は、上記姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋と、上記速度増分計算部１２６が計算した速度の増分Δｖｎと、上記速度減算部１３０が計算した速度の差ｄｖとに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを推定する。
【００８７】
この実施の形態における航法装置１００によれば、第一推定部１４０が推定した角速度の誤差ｂｇと姿勢角の誤差εとに基づいて、角速度計測部（角速度センサ１１１）が計測した角速度ωｂを角速度誤差補正部１２１が補正し、姿勢角外挿計算部１２２が計算した姿勢角ｑｎを姿勢角誤差補正部１２３が補正するので、移動体８００の状態を精度よく推定することができる。
【００８８】
この実施の形態における航法装置１００において、上記第一推定部１４０は、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを状態量とするカルマンフィルタにより、上記状態量を推定する。
【００８９】
この実施の形態における航法装置１００によれば、第一推定部１４０がカルマンフィルタにより状態量を推定するので、状態量の成分が少ないことにより計算量が大幅に削減できる。
【００９０】
この実施の形態における航法装置１００において、上記第一推定部１４０は、以下の式で表される観測量ベクトルΔｚと、状態量ベクトルｘと、推移行列Φと、観測行列Ｈとを用いたカルマンフィルタにより上記状態量を推定する。
【数２８】
ただし、ｄｖは、上記速度減算部１３０が計算した速度の差を表わす三次の縦ベクトル。εは、上記第一推定部１４０が推定する角速度の誤差を表わす三次の縦ベクトル。ｂｇは、上記第一推定部１４０が推定する姿勢角の誤差を表わす三次の縦ベクトル。Ｉ３×３は、三行三列の単位行列。０３×３は、三行三列の零行列。Ｔｓは、上記速度計測部（ＧＰＳ受信機１１３）が速度を計測する周期を表わすスカラー値。Ｃｎｂは、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎに基づいて、上記第一座標系を、上記第二座標系へ変換する座標変換を表わす三行三列の方向余弦行列。Δｖｎ＝［Δｖｎｘ Δｖｎｙ Δｖｎｚ］は、上記速度増分計算部１２６が計算した速度の増分を表わす三次のベクトル。
【００９１】
この実施の形態における航法装置１００によれば、加速度計測部（加速度センサ１１２）が計測した第一座標系（移動体座標系）における加速度ａｂを、姿勢角外挿計算部１２２が計算した姿勢角ｑｎに基づいて、速度増分計算部１２６が座標変換し、座標変換した加速度に基づいて速度の増分Δｖｎを算出し、算出した速度の増分Δｖｎの成分を、観測行列Ｈの成分とするので、推定すべき状態量である姿勢角の誤差εと、観測量である速度の差ｄｖとを結びつけることができ、速度の誤差などを状態量として推定する必要がないので、カルマンフィルタで使用する各行列の次数を小さくすることができ、カルマンフィルタの計算に必要な計算量を少なくすることができる。、
【００９２】
この実施の形態における航法装置１００は、更に、速度誤差推定部１５２と、速度誤差補正部１２８とを有する。
上記速度誤差推定部１５２は、上記速度減算部１３０が計算した速度の差ｄｖに基づいて、上記速度外挿計算部１２７が計算した速度の誤差δｖを推定する。
上記速度誤差補正部１２８は、上記速度誤差推定部１５２が推定した誤差δｖに基づいて、上記速度外挿計算部１２７が計算した速度ｖｎを補正する。
【００９３】
この実施の形態における航法装置１００によれば、速度減算部１３０が計算した速度の差ｄｖに基づいて、速度誤差推定部１５２が速度の誤差δｖを推定するので、カルマンフィルタのような計算量の多い推定方式を用いる必要がなく、推定に必要な計算の計算量を少なくすることができる。
【００９４】
この実施の形態における航法装置１００において、上記速度誤差推定部１５２は、上記速度減算部１３０が計算した速度の差ｄｖに、所定のゲインＫｖを乗じ、上記速度外挿計算部１２７が計算した速度の誤差δｖとする。
【００９５】
この実施の形態における航法装置１００によれば、速度減算部１３０が計算した速度の差ｄｖに、速度誤差推定部１５２が所定のゲインＫｖを乗じることにより、速度の誤差δｖを推定するので、極めて少ない計算量で、速度の誤差δｖを推定することができる。
【００９６】
この実施の形態における航法装置１００は、更に、速度計測精度算出部（ＧＰＳ受信機１１３）と、ゲイン算出部１５１とを有する。
上記速度計測精度算出部（ＧＰＳ受信機１１３）は、上記速度計測部（ＧＰＳ受信機１１３）が計測した速度の精度を算出する。
上記ゲイン算出部１５１は、上記速度計測精度算出部（ＧＰＳ受信機１１３）が算出した精度に基づいて、ゲインＫｖを算出する。
上記速度誤差推定部１５２は、上記速度減算部１３０が計算した速度の差ｄｖに、上記ゲイン算出部１５１が算出したゲインＫｖを乗じ、上記速度外挿計算部１２７が計算した速度の誤差δｖとする。
【００９７】
この実施の形態における航法装置１００によれば、速度計測精度算出部（ＧＰＳ受信機１１３）が算出した精度に基づいて、ゲイン算出部１５１がゲインＫｖを算出するので、速度計測部（ＧＰＳ受信機１１３）の計測精度に応じて、ゲインＫｖを調整することができ、速度の誤差δｖの推定の妥当性を高くすることができる。
【００９８】
この実施の形態における航法装置１００が、推定をする推定方法は、以下の工程を有する。
上記角速度計測部（角速度センサ１１１）が、上記航法装置１００が搭載された移動体８００を基準とした第一座標系（移動体座標系）における上記移動体８００の角速度ωｂを計測する。
上記加速度計測部（加速度センサ１１２）が、上記第一座標系における上記移動体８００に加わる加速度ａｂを計測する。
上記速度計測部（ＧＰＳ受信機１１３）が、上記第一座標系と異なる第二座標系（慣性座標系）における上記移動体８００の速度ｖｇを計測する。
上記姿勢角外挿計算部１２２が、上記角速度計測部（角速度センサ１１１）が計測した角速度ωｂに基づいて、上記第二座標系における上記移動体８００の姿勢角ｑｎを計算する。
上記速度増分計算部１２６が、上記加速度計測部（加速度センサ１１２）が計測した加速度ａｂと、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎとに基づいて、上記第二座標系における上記移動体８００の速度の増分Δｖｎを計算する。
上記速度外挿計算部１２７が、上記速度増分計算部１２６が計算した速度の増分Δｖｎに基づいて、上記第二座標系における上記移動体８００の速度ｖｎを計算する。
上記速度減算部１３０が、上記速度外挿計算部１２７が計算した速度ｖｎと、上記速度計測部（ＧＰＳ受信機１１３）が計測した速度ｖｇとの差ｄｖを計算する。
上記第一推定部１４０が、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、上記速度増分計算部１２６が計算した速度の増分Δｖｎと、上記速度減算部１３０が計算した速度の差ｄｖとに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを推定する。
【００９９】
この実施の形態における推定方法によれば、姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、速度増分計算部１２６が計算した速度の増分Δｖｎと、速度減算部１３０が計算した速度の差ｄｖとに基づいて、第一推定部１４０が角速度の誤差ｂｇと、姿勢角の誤差εとを推定するので、推定にかかる計算量を少なくすることができる。
【０１００】
以上説明した航法装置１００は、航空機等の加速度運動を伴う移動体８００に搭載され、移動体８００の三次元姿勢角ｑ及び速度ｖの推定を行う。航法装置１００は、ＧＰＳ速度出力ｖｇと慣性航法計算に基づく速度ｖｎとの差ｄｖを観測量Δｚとし、姿勢角誤差εおよび角速度バイアス値ｂｇを推定・補正する推定フィルタ（第一推定部１４０）と、同じく観測量（速度の差ｄｖ）に基づき、速度誤差δｖを推定・補正する第２の推定フィルタ（第二推定部１５０）により、推定フィルタを構成する。これにより、従来方式に比べて著しく推定精度を劣化させることなく、計算負荷を低減することができる。
【０１０１】
航法装置１００は、カルマンフィルタにおける行列計算において、行列の次数を少なくすることにより、計算負荷を低くし、廉価な計算機により航法装置１００を構成できる。これにより、例えば、組込製品等で使用される廉価なマイクロコントローラに実装することができる。
【０１０２】
ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）（角速度計測部・加速度計測部）は、角速度センサと加速度センサをそれぞれ３軸搭載する。ＩＭＵは、直交座標系として定義された移動体の機体座標系（第一座標系）における角速度ベクトルωｂ、加速度ベクトルａｂを計測し、慣性航法部１２０に出力する。
慣性航法部１２０は、適当なアライメント処理およびＧＰＳ等により、速度、姿勢角の初期化を行う。その後、慣性航法部１２０は、ストラップダウン航法計算（外挿計算処理）に基づき、速度ｖｎ、姿勢角ｑｎの更新を行い、出力する。なお、姿勢角の表現形式として、コータニオンに基づく表現形式を例として用いたが、オイラー角、方向余弦行列等、他の表現形式を用いる構成であってもよい。また、ストラップダウン航法計算の計算方式は、上述した計算式によるものに限らず、他の計算方式を用いる構成であってもよい。
【０１０３】
慣性航法部１２０が計算する速度ｖｎ、姿勢角ｑｎは、それぞれの初期化誤差や、ＩＭＵが計測した角速度ベクトルωｂ、加速度ベクトルａｂに含まれるバイアス誤差等の要因により、誤差を発生する。ＩＭＵの計測誤差は積分され、速度や姿勢角の誤差は、時間と共に増加する傾向がある。
【０１０４】
ＧＰＳ受信機１１３は、ＧＰＳ測位に基づく速度ｖｇを出力する。減算器（速度減算部１３０）は、ＧＰＳ受信機１１３の速度出力ｖｇから慣性航法部１２０の速度出力ｖｎを差し引いた差ｄｖを計算する。
第一推定部１４０は、速度差ｄｖと速度出力ｖｎ、姿勢角出力ｑｎから６次元のカルマンフィルタ計算に基づき姿勢角ｑｎの補正εおよび角速度バイアス補正量ｂｇを推定する。
第二推定部１５０は、速度差ｄｖと航法計算部（慣性航法部１２０）の速度出力ｖｎから速度出力ｖｎの誤差を算出し、速度誤差補正部１２８は、速度出力ｖｎの補正を行う。
【０１０５】
第一推定部１４０におけるカルマンフィルタ計算は、状態量として、位置ｐｎ、速度ｖｎ、加速度バイアス補正量ｂａを含んでいない。また、観測量として、位置の差分ｄｐを含んでいない。
航法装置１００は、従来よりも状態量の数を少なくしているが、推定精度の劣化は小さく、実用的に問題がない精度を確保することができる。なぜなら、姿勢角誤差εを推定する基本的な原理は不変であり、廉価な加速度センサにおいても比較的バイアス安定度は高く、姿勢角決定精度は角速度センサの性能に基づくため、加速度センサのバイアス補正量ｂａを省略し、温度補正のみを行った場合でも、性能の劣化を小さく抑えることができるからである。
【０１０６】
第二推定部１５０は、速度の誤差δｖを推定し、速度誤差補正部１２８が速度ｖｎを補正する処理を行う。この計算において、第二推定部１５０は、ゲインＫｖを、例えば、固定、または、ＧＰＳ測位条件に基づいてテーブルにより設定する。これにより、第二推定部１５０における計算負荷は、第一推定部１４０における計算負荷と比べて無視できるレベルとすることができる。
【０１０７】
カルマンフィルタにおける計算負荷は、状態量の次元ｎと観測量の次元ｒに依存する。例えば、状態量の次元ｎを５分の２（１５→６）、観測量の次元ｒを２分の１（６→３）とすれば、１ステップあたりの乗算の数は、１０％以下と著しく減少する。このため、実装において、性能が比較的低いマイクロコンピュータを使用することができ、廉価なシステムを構成することができる。
【０１０８】
角速度計測部（角速度センサ１１１）が計測した角速度ωｂの誤差は、外挿計算により蓄積し、姿勢角外挿計算部１２２が算出する姿勢角ｑｎの誤差に反映する。姿勢角外挿計算部１２２が算出する姿勢角ｑｎの誤差は、外挿計算により蓄積し、速度外挿計算部１２７が算出する速度ｖｎの誤差に反映する。
したがって、角速度ωｂの誤差が姿勢角ｑｎの誤差に反映するのは比較的速いのに対し、姿勢角ｑｎを介する分、角速度ωｂの誤差が速度ｖｎの誤差に反映するのは比較的遅い。このように、誤差の反映する速さが異なる観測量に基づいて、一つのカルマンフィルタで推測しようとすることは、効率が悪い。
以上説明した航法装置１００は、誤差が反映するのが比較的速い部分に絞って、カルマンフィルタによる推測を行うことにより、効率を高くし、必要な計算量を少なくする。
【０１０９】
実施の形態２．
実施の形態２について、図９〜図１４を用いて説明する。
なお、実施の形態１で説明した部分と共通する部分については、同一の符号を付し、説明を省略する場合がある。
【０１１０】
図９は、この実施の形態における航法装置１００の機能ブロックの構成の一例を示すブロック構成図である。
航法装置１００は、実施の形態１で説明した機能ブロックに加えて、更に、位置減算部１６０を有する。
【０１１１】
慣性航法部１２０は、更に、推定した速度ｖｎに基づいて、慣性座標系における移動体８００の位置ｐｎを推定する。
位置減算部１６０は、ＧＰＳ受信機１１３が計測した位置ｐｇと、慣性航法部１２０が推定した位置ｐｎとの差ｄｐ＝ｐｇ−ｐｎを計算する。ここで、位置ｐｇ及び位置ｐｎは、ともに三次の縦ベクトルであるから、位置減算部１６０が計算する位置の差ｄｐも三次の縦ベクトルである。位置減算部１６０は、計算した速度の差ｄｐを表わすデータを出力する。
【０１１２】
第二推定部１５０は、更に、位置減算部１６０が計算した位置の差ｄｐと、ＧＰＳ受信機１１３が推定した精度とに基づいて、慣性航法部１２０が推定した位置ｐｎの誤差δｐを推定する。なお、第二推定部１５０が推定する位置ｐｎの誤差δｐは、例えば、慣性座標系のｘｙｚ三軸それぞれについての誤差を成分とする三次の縦ベクトルである。第二推定部１５０は、更に、推定した位置の誤差δｐを表わすデータを出力する。
【０１１３】
慣性航法部１２０は、更に、第二推定部１５０が推定した位置の誤差δｐに基づいて、推定した位置ｐｎを補正し、誤差を補正した位置ｐを算出する。慣性航法部１２０は、更に、算出した慣性座標系における位置ｐを表わすデータを出力する。
【０１１４】
位置出力部１９３は、慣性航法部１２０が出力した位置ｐを表わすデータを入力する。位置出力部１９３は、入力したデータが表わす位置ｐを、外部に出力する。
【０１１５】
図１０は、この実施の形態における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図である。
慣性航法部１２０は、実施の形態１で説明した機能ブロックに加えて、更に、位置外挿計算部１７１、位置誤差補正部１７２、位置記憶部１７３を有する。
【０１１６】
位置記憶部１７３は、慣性座標系における移動体８００の位置ｐを表わすデータを記憶している。位置記憶部１７３は、例えば、位置ｐを表わすデータとして、慣性座標系におけるｘｙｚ三軸それぞれの座標を表わす三つの実数ｐｘ，ｐｙ，ｐｚを表わすデータを記憶する。
位置記憶部１７３は、あらかじめ、例えば、位置が既知のマーカー上で移動体８００が静止しているときに、マーカーの位置を用いて、記憶した位置ｐを表わすデータを初期化しておく。
【０１１７】
位置外挿計算部１７１は、位置記憶部１７３が記憶した慣性座標系における位置ｐを表わすデータと、速度誤差補正部１２８が出力した補正した速度ｖｎ＋を表わすデータとを入力する。位置外挿計算部１７１は、入力したデータが表わす位置ｐと速度ｖｎ＋とに基づいて、外挿計算をし、慣性座標系における新しい位置ｐｎを算出する。位置外挿計算部１７１は、算出した位置ｐｎを表わすデータを出力する。位置外挿計算部１７１が出力した位置ｐｎを表わすデータは、位置減算部１６０が入力し、位置の差ｄｐの算出に使用する。
位置外挿計算部１７１は、例えば、次の計算式を用いて、位置ｐｎを算出する。
【数２９】
ここで、ｐｎｋは、位置外挿計算部１７１が算出する時刻ｔｋにおける位置を表わす三次の縦ベクトル。ｐｋ−１は、位置記憶部１７３が記憶した時刻ｔｋ−１における位置を表わす三次の縦ベクトル。Ｔｓは、角速度センサ１１１及び加速度センサ１１２が角速度や加速度を計測する周期を表わすスカラー値で、数１２や数１５や数２０におけるＴｓと同じく、Ｔｓ＝ｔｋ−ｔｋ−１。ｖｎ＋ｋは、速度誤差補正部１２８が補正した時刻ｋにおける速度を表わす三次の縦ベクトル。
【０１１８】
位置誤差補正部１７２は、位置外挿計算部１７１が出力した慣性座標系における位置ｐｎを表わすデータと、第二推定部１５０が出力した位置の誤差δｐを表わすデータとを入力する。位置誤差補正部１７２は、入力したデータが表わす位置ｐｎと位置の誤差δｐとに基づいて、位置ｐｎの誤差を補正して、補正した位置ｐｎ＋を算出する。位置誤差補正部１７２は、算出した位置ｐｎ＋を表わすデータを出力する。
位置誤差補正部１７２は、例えば、次の計算式を用いて、補正した位置ｐｎ＋を算出する。
【数３０】
ここで、ｐｎ＋ｋは、位置誤差補正部１７２が算出する時刻ｔｋにおける補正した位置を表わす三次の縦ベクトル。ｐｎｋは、位置外挿計算部１７１が算出した時刻ｔｋにおける位置を表わす三次の縦ベクトル。δｐは、第二推定部１５０が推定した位置の誤差を表わす三次の縦ベクトル。
【０１１９】
なお、ＧＰＳ受信機１１３が移動体８００の位置・速度を計測していない場合、位置誤差補正部１７２は、位置誤差の補正をせず、入力したデータが表わす位置ｐｎを、そのまま位置ｐｎ＋とする。
【０１２０】
位置記憶部１７３は、位置誤差補正部１７２が出力した補正した位置ｐｎ＋を表わすデータを入力する。位置記憶部１７３は、入力したデータを、位置ｐを表わすデータとして記憶する。位置記憶部１７３が記憶した位置ｐを表わすデータは、位置出力部１９３が入力して、外部に出力する。
【０１２１】
図１１は、この実施の形態における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図である。
第二推定部１５０は、実施の形態１で説明した機能ブロックに加えて、更に、位置誤差推定部１５３を有する。
【０１２２】
ゲイン算出部１５１は、更に、ＧＰＳ受信機１１３が推定した位置の計測の精度に基づいて、位置誤差のゲインＫｐを算出する。ゲイン算出部１５１は、算出したゲインＫｐを表わすデータを出力する。
ゲイン算出部１５１が算出するゲインＫｐは、ゲインＫｖと同様の値であり、ゲインＫｖと同じであってもよいし、異なっていてもよい。
【０１２３】
位置誤差推定部１５３は、位置減算部１６０が出力した位置の差ｄｐを表わすデータと、ゲイン算出部１５１が出力したゲインＫｐを表わすデータとを入力する。位置誤差推定部１５３は、入力したデータが表わす位置の差ｄｐとゲインＫｐとに基づいて、慣性航法部１２０が推定した位置の誤差δｐを算出する。位置誤差推定部１５３は、算出した位置の誤差δｐを表わすデータを出力する。位置誤差推定部１５３が出力した位置の誤差δｐを表わすデータは、慣性航法部１２０が入力し、誤差の補正に使用する。
位置誤差推定部１５３は、例えば、次の計算式を用いて、位置の誤差δｐを算出する。
【数３１】
ここで、δｐは、位置誤差推定部１５３が算出する位置の誤差を表わす三次の縦ベクトル。Ｋｐは、ゲイン算出部１５１が算出した位置誤差のゲインを表わすスカラー値もしくは三行三列の行列。ｄｐは、位置減算部１６０が算出した位置の差を表わす三次の縦ベクトル。
【０１２４】
次に、動作について説明する。
この実施の形態における航法処理の全体的な流れは、実施の形態１で説明したものと同様である。
【０１２５】
図１２は、この実施の形態における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図である。
初期化処理Ｓ５１０は、実施の形態１で説明した工程に加えて、更に、位置初期化工程Ｓ５１３を有する。
【０１２６】
位置初期化工程Ｓ５１３において、位置記憶部１７３は、記憶した位置ｐを初期化する。
【０１２７】
図１３は、この実施の形態における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図である。
外挿計算処理Ｓ５２０は、実施の形態１で説明した工程に加えて、更に、位置外挿計算工程Ｓ５２７を有する。
【０１２８】
位置外挿計算工程Ｓ５２７において、位置外挿計算部１７１は、速度外挿計算工程Ｓ５２６で速度外挿計算部１２７が算出した速度ｖｎに基づいて、位置記憶部１７３が記憶した位置ｐを外挿し、外挿した位置ｐｎを算出する。ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、位置外挿計算部１７１が算出した外挿した位置ｐｎが、そのまま位置記憶部１７３が記憶する位置ｐとなるので、位置記憶部１７３は、位置外挿計算部１７１が外挿した位置ｐｎを、暫定的に、位置ｐとして記憶する。
【０１２９】
図１４は、この実施の形態における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図である。
観測更新処理Ｓ５５０は、実施の形態１で説明した工程に加えて、更に、位置減算工程Ｓ５６４、位置誤差推定工程Ｓ５６５、位置誤差補正工程Ｓ５６６を有する。
【０１３０】
ゲイン算出工程Ｓ５６１において、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した計測の精度に基づいて、速度誤差のゲインＫｖと、位置誤差のゲインＫｐとを算出する。
【０１３１】
位置減算工程Ｓ５６４において、位置減算部１６０は、ＧＰＳ受信機１１３が計測した位置ｐｇと、位置外挿計算工程Ｓ５２７で位置外挿計算部１７１が外挿した位置ｐｎとに基づいて、位置の差ｄｐを算出する。
位置誤差推定工程Ｓ５６５において、位置誤差推定部１５３は、ゲイン算出工程Ｓ５６１でゲイン算出部１５１が算出した位置誤差のゲインＫｐと、位置減算工程Ｓ５６４で位置減算部１６０が算出した位置の差ｄｐとに基づいて、位置の誤差δｐを算出する。
位置誤差補正工程Ｓ５６６において、位置誤差補正部１７２は、位置誤差推定工程Ｓ５６５で位置誤差推定部１５３が算出した位置の誤差δｐに基づいて、位置外挿計算工程Ｓ５２７で位置外挿計算部１７１が外挿した位置ｐｎを補正し、補正した位置ｐｎ＋を算出する。位置記憶部１７３は、位置誤差補正部１７２が補正した位置ｐｎ＋を、位置ｐとして記憶する。
なお、ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、観測更新処理Ｓ５５０を実行しないので、位置記憶部１７３は、位置外挿計算工程Ｓ５２７で暫定的に記憶した位置ｐ（すなわち、位置外挿計算部１７１が外挿した位置ｐｎ）を、そのまま記憶し続けることになる。
【０１３２】
この実施の形態における航法装置１００は、更に、位置計測部（ＧＰＳ受信機１１３）と、位置外挿計算部１７１と、位置減算部１６０と、位置誤差推定部１５３と、位置誤差補正部１７２とを有する。
上記位置計測部（ＧＰＳ受信機１１３）は、上記第二座標系（慣性座標系）における移動体８００の位置ｐｇを計測する。
上記位置外挿計算部１７１は、上記速度外挿計算部１２７が計算した速度ｖｎに基づいて、上記第二座標系における移動体８００の位置ｐｎを計算する。
上記位置減算部１６０は、上記位置外挿計算部１７１が計算した位置ｐｎと、上記位置計測部（ＧＰＳ受信機１１３）が計測した位置ｐｇとの差を計算する。
上記位置誤差推定部１５３は、上記位置減算部１６０が計算した位置の差ｄｐに基づいて、上記位置外挿計算部１７１が計算した位置の誤差δｐを推定する。
上記位置誤差補正部１７２は、上記位置誤差推定部１５３が推定した位置の誤差δｐに基づいて、上記位置外挿計算部１７１が計算した位置ｐｎを補正する。
【０１３３】
この実施の形態における航法装置１００によれば、速度外挿計算部１２７が算出した移動体８００の速度ｖｎに基づいて、位置外挿計算部１７１が移動体８００の位置ｐｎを算出するので、なんらかの原因で、位置計測部（ＧＰＳ受信機１１３）が移動体８００の位置ｐｇを計測できない場合であっても、移動体８００の位置を推定することができる。
【０１３４】
以上説明した航法装置１００は、慣性航法部１２０において位置外挿計算を実施し、第二推定部１５０において位置に関する更新計算も行う。航法装置１００は、慣性航法部１２０が、位置外挿計算を実施し、減算器（速度減算部１３０・位置減算部１６０）が、位置・速度の差ｄｐ、ｄｖを計算し、第二推定部１５０が、位置に関する更新計算も行う。なお、位置誤差のゲインＫｐは、速度誤差のゲインＫｖと同様に固定または簡単な計算により設定することができる。
これにより、ＧＰＳ出力が一時中断した場合やＧＰＳ出力の遅延が大きいことが問題となる場合など、慣性装置に基づく位置出力ｐｎを使用することができる。
【図面の簡単な説明】
【０１３５】
【図１】実施の形態１における航法装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図２】実施の形態１における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図。
【図３】実施の形態１における第一推定部１４０の機能ブロックの構成の一例を示す詳細ブロック図。
【図４】実施の形態１における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図。
【図５】実施の形態１における航法装置１００が移動体８００の速度・姿勢角を推定する航法処理の全体的な流れの一例を示すフローチャート図。
【図６】実施の形態１における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図。
【図７】実施の形態１における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図。
【図８】実施の形態１における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図。
【図９】実施の形態２における航法装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図１０】実施の形態２における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図。
【図１１】実施の形態２における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図。
【図１２】実施の形態２における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図。
【図１３】実施の形態２における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図。
【図１４】実施の形態２における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図。
【符号の説明】
【０１３６】
１００ 航法装置、１１１ 角速度センサ、１１２ 加速度センサ、１１３ ＧＰＳ受信機、１２０ 慣性航法部、１２１ 角速度誤差補正部、１２２ 姿勢角外挿計算部、１２３ 姿勢角誤差補正部、１２４ 姿勢角記憶部、１２５ 方向余弦行列計算部、１２６ 速度増分計算部、１２７ 速度外挿計算部、１２８ 速度誤差補正部、１２９ 速度記憶部、１３０ 速度減算部、１４０ 第一推定部、１４１ 観測行列計算部、１４２ 推移行列計算部、１４３ 誤差共分散外挿計算部、１４４ カルマンゲイン計算部、１４５ 誤差共分散観測更新計算部、１４６ 誤差共分散記憶部、１４７ 状態量外挿計算部、１４８ 状態量観測更新計算部、１４９ 状態量記憶部、１５０ 第二推定部、１５１ ゲイン算出部、１５２ 速度誤差推定部、１５３ 位置誤差推定部、１６０ 位置減算部、１７１ 位置外挿計算部、１７２ 位置誤差補正部、１７３ 位置記憶部、１９１ 姿勢角出力部、１９２ 速度出力部、１９３ 位置出力部、８００ 移動体。
【技術分野】
【０００１】
この発明は、航空機等の移動体に搭載され、移動体の３次元姿勢角や速度の推定を行う航法装置及びその推定方法に関する。
【背景技術】
【０００２】
航法装置は、慣性センサである角速度センサおよび加速度センサの出力を積分することにより、移動体の姿勢角および速度を得る。特に、安定度の低い廉価な慣性センサを用いた廉価な航法装置の場合、出力に含まれるバイアス誤差等の誤差要因により、時間と共に急激に精度が劣化するため、ＧＰＳ受信機の位置または速度出力により、誤差を補正する機能を有している場合が多い。この誤差の補正手段としては、カルマンフィルタ等の推定器が用いられる。
【非特許文献１】廣川類、佐藤幸一、中久喜健司「Ｄｅｓｉｇｎ ａｎｄ Ｅｖａｌｕａｔｉｏｎ ｏｆ Ａ Ｔｉｇｈｔｌｙ Ｃｏｕｐｌｅｄ ＧＰＳ／ＩＮＳ ｕｓｉｎｇ Ｌｏｗ Ｃｏｓｔ ＭＥＭＳ ＩＭＵ」ＧＰＳ／ＧＮＳＳ国際シンポジウム２００３、２００３年。
【非特許文献２】Ｊａｙ Ａ．Ｆａｒｒｅｌｌ、Ｍａｔｔｈｅｗ Ｂａｒｔｈ「Ｔｈｅ Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ ａｎｄ Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ」ＭｃＧｒａｗ−Ｈｉｌｌ、１９９９年。
【発明の開示】
【発明が解決しようとする課題】
【０００３】
航空機等、三次元空間で加速度運動をする移動体の場合、カルマンフィルタで高次の行列計算を多く行う必要があるため、計算負荷が高い。そのため、高い処理能力が必要となり、航法装置の製造コストが高くなるという課題がある。
この発明は、例えば、上記のような課題を解決するためになされたものであり、推定の精度を劣化させることなく、航法装置における計算負荷を低減することにより、航法装置の製造コストを低くすることを目的とする。
【課題を解決するための手段】
【０００４】
この発明にかかる航法装置は、
移動体に搭載される航法装置において、
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有し、
上記角速度計測部は、上記移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部は、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部は、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部は、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部は、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部は、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部は、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする。
【発明の効果】
【０００５】
この発明にかかる航法装置によれば、姿勢角外挿計算部が計算した姿勢角ｑｎと、速度増分計算部が計算した速度の増分Δｖｎと、速度減算部が計算した速度の差ｄｖとに基づいて、第一推定部が角速度の誤差ｂｇと、姿勢角の誤差εとを推定するので、推定にかかる計算量が少なく、航法装置１００の製造コストを低く抑えることができる。
【発明を実施するための最良の形態】
【０００６】
実施の形態１．
実施の形態１について、図１〜図８を用いて説明する。
【０００７】
図１は、この実施の形態における航法装置１００の機能ブロックの構成の一例を示すブロック構成図である。
航法装置１００は、航空機や車両などの移動体８００に搭載され、移動体８００の位置、速度、姿勢角などを推定する装置である。
航法装置１００は、角速度センサ１１１、加速度センサ１１２、ＧＰＳ受信機１１３、慣性航法部１２０、速度減算部１３０、第一推定部１４０、第二推定部１５０、姿勢角出力部１９１、速度出力部１９２、位置出力部１９３を有する。
【０００８】
なお、以下の説明において、移動体８００を基準とする移動体座標系（第一座標系）の直交軸を、大文字でＸ軸・Ｙ軸・Ｚ軸、地球を基準とする慣性座標系（第二座標系）の直交軸を小文字でｘ軸・ｙ軸・ｚ軸と表記する。
移動体座標系は、例えば、移動体８００の重心を原点とし、移動体８００の前方向をＸ軸、移動体８００の右方向をＹ軸、移動体８００の下方向をＺ軸とする。
慣性座標系は、例えば、地球上の所定の点を原点とし、北方向をｘ軸、東方向をｙ軸、下方向をｚ軸とする。あるいは、慣性座標系は、地球の中心を原点とし、春分点方向をｘ軸、赤道面内で春分点方向に対して垂直な方向をｙ軸、地軸方向をｚ軸としてもよい。
【０００９】
角速度センサ１１１（角速度計測部）は、例えば、ジャイロスコープなど、移動体８００の角速度を計測するセンサである。角速度センサ１１１は、Ｘ軸を中心とする回転の角速度ωＸと、Ｙ軸を中心とする回転の角速度ωＹと、Ｚ軸を中心とする回転の角速度ωＺとの三つの角速度を計測する。以下、角速度センサ１１１が計測した移動体座標系における三軸の角速度ωＸ，ωＹ，ωＺを成分とする三次の縦ベクトルを「ωｂ」（＝［ωＸ ωＹ ωＺ］Ｔ）と表記し、角速度センサ１１１が時刻ｔｋに計測した角速度ωｂを「ωｂｋ」と表記する。角速度センサ１１１は、計測した角速度ωｂを表わすデータを出力する。角速度センサ１１１が計測した角速度ωｂを表わすデータを出力する頻度は、例えば、１秒間に１００回程度である。
【００１０】
加速度センサ１１２（加速度計測部）は、移動体８００に加わる加速度を計測するセンサである。加速度センサ１１２は、Ｘ軸方向の加速度ａＸと、Ｙ軸方向の加速度ａＹと、Ｚ軸方向の加速度ａＺとの三つの加速度を計測する。以下、加速度センサ１１２が計測した移動対座標系における三軸の加速度ａＸ，ａＹ，ａＺを成分とする三次の縦ベクトルを「ａｂ」（＝［ａＸ ａＹ ａＺ］Ｔ）と表記し、加速度センサ１１２が時刻ｔｋに計測した加速度ａｂを「ａｂｋ」と表記する。加速度センサ１１２は、計測した加速度ａｂを表わすデータを出力する。加速度センサ１１２が計測した加速度ａｂを表わすデータを出力する頻度は、角速度センサ１１１と同様、例えば、１秒間に１００回程度である。
【００１１】
ＧＰＳ受信機１１３（速度計測部・位置計測部・速度計測精度算出部）は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）衛星が送信した電波を受信することにより、移動体８００の位置ｐｇや速度ｖｇなどを計測する。ＧＰＳ受信機１１３が計測する位置ｐｇや速度ｖｇは、慣性座標系におけるものであり、それぞれｘｙｚ軸方向の成分を有する。以下の説明において、ＧＰＳ受信機１１３が計測した位置ｐｇや速度ｖｇは、三次の縦ベクトルとして扱う。また、ＧＰＳ受信機１１３が時刻ｔｋに計測した位置ｐｇ及び速度ｖｇをそれぞれ「ｐｇｋ」「ｖｇｋ」と表記する。ＧＰＳ受信機１１３は、計測した位置ｐｇや速度ｖｇを表わすデータを出力する。ＧＰＳ受信機１１３が計測した位置ｐｇや速度ｖｇを表わすデータを出力する頻度は、例えば、１秒間に１〜４回程度である。
また、ＧＰＳ受信機１１３は、受信した電波の数や、ＧＰＳ衛星の配置などに基づいて、計測した位置ｐｇや速度ｐｇの精度を推定する。ＧＰＳ受信機１１３は、推定した精度を表わすデータを出力する。
【００１２】
慣性航法部１２０は、角速度センサ１１１が計測した角速度ωｂや加速度センサ１１２が計測した加速度ａｂに基づいて、慣性座標系における移動体８００の姿勢角ｑｎや速度ｖｎを推定する。推定の詳細については、後述する。
慣性航法部１２０は、例えば、データを処理するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの処理装置が、プログラムを実行することにより実現してもよいし、ハードウェアにより実現してもよい。あるいは、ハードウェアとソフトウェアとの組み合わせにより実現してもよい。速度減算部１３０、第一推定部１４０、第二推定部１５０なども同様である。
【００１３】
速度減算部１３０は、ＧＰＳ受信機１１３が計測した速度ｖｇと、慣性航法部１２０が推定した速度ｖｎとの差ｄｖ＝ｖｇ−ｖｎを計算する。ここで、速度ｖｇ及び速度ｖｎは、ともに三次の縦ベクトルであるから、速度減算部１３０が計算する速度の差ｄｖも三次の縦ベクトルである。速度減算部１３０は、計算した速度の差ｄｖを表わすデータを出力する。
【００１４】
第一推定部１４０は、慣性航法部１２０が推定した姿勢角ｑｎや速度ｖｎと、速度減算部１３０が計算した速度の差ｄｖとに基づいて、角速度センサ１１１が計測した角速度ωｂのバイアス誤差ｂｇや慣性航法部１２０が推定した姿勢角ｑｎの誤差εを推定する。なお、第一推定部１４０が推定する角速度ωｂのバイアス誤差ｂｇは、例えば、角速度ωｂの三つの成分に対応して、移動体座標系のＸＹＺ三軸それぞれについてのバイアス誤差を成分とする三次の縦ベクトルである。また、第一推定部１４０が推定する姿勢角ｑｎの誤差εは、例えば、慣性座標系のｘｙｚ三軸それぞれを中心とする回転誤差を成分とする三次の縦ベクトルである。なお、推定の詳細については、後述する。第一推定部１４０は、推定した角速度ωｂのバイアス誤差ｂｇや姿勢角ｑｎの誤差εを表わすデータを出力する。
【００１５】
第二推定部１５０は、速度減算部１３０が計算した速度の差ｄｖと、ＧＰＳ受信機１１３が推定した精度とに基づいて、慣性航法部１２０が推定した速度ｖｎの誤差δｖを推定する。なお、第二推定部１５０が推定する速度ｖｎの誤差δｖは、例えば、慣性座標系のｘｙｚ三軸それぞれについての誤差を成分とする三次の縦ベクトルである。推定の詳細ついては、後述する。第二推定部１５０は、推定した速度の誤差δｖを表わすデータを出力する。
【００１６】
慣性航法部１２０は、第一推定部１４０や第二推定部１５０が推定した誤差に基づいて、誤差の補正を行い、誤差を補正した姿勢角ｑや速度ｖを算出する。慣性航法部１２０は、算出した慣性座標系における姿勢角ｑや速度ｖを表わすデータを出力する。
【００１７】
姿勢角出力部１９１は、慣性航法部１２０が出力した姿勢角ｑを表わすデータを入力する。姿勢角出力部１９１は、入力したデータが表わす姿勢角ｑを、例えば、表示装置を用いて表示したり、送信装置を用いて送信したりするなどして、外部に出力する。
【００１８】
速度出力部１９２は、慣性航法部１２０が出力した速度ｖを表わすデータを入力する。速度出力部１９２は、姿勢角出力部１９１と同様、入力したデータが表わす速度ｖを、外部に出力する。
【００１９】
位置出力部１９３は、ＧＰＳ受信機１１３が出力した位置ｐｇを表わすデータを入力する。位置出力部１９３は、姿勢角出力部１９１と同様、入力したデータが表わす位置ｐｇを、外部に出力する。
【００２０】
図２は、この実施の形態における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図である。
慣性航法部１２０は、角速度誤差補正部１２１、姿勢角外挿計算部１２２、姿勢角誤差補正部１２３、姿勢角記憶部１２４、方向余弦行列計算部１２５、速度増分計算部１２６、速度外挿計算部１２７、速度誤差補正部１２８、速度記憶部１２９を有する。
【００２１】
角速度誤差補正部１２１は、角速度センサ１１１が出力した角速度ωｂを表わすデータと、第一推定部１４０が出力した角速度ωｂのバイアス誤差ｂｇを表わすデータとを入力する。角速度誤差補正部１２１は、入力したデータが表わす角速度ωｂと、バイアス誤差ｂｇとに基づいて、角速度ωｂの誤差を補正して、誤差を補正した角速度ωｂ＋を算出する。角速度誤差補正部１２１が算出する角速度ωｂ＋は、角速度ωｂと同様、移動体座標系におけるＸＹＺ三軸それぞれを中心とする角速度を成分とする三次の縦ベクトルである。角速度誤差補正部１２１は、算出した角速度ωｂ＋を表わすデータを出力する。
角速度誤差補正部１２１は、例えば、次の計算式を用いて、角速度ωｂ＋を算出する。
【数１１】
ここで、ωｂ＋ｋは、角速度誤差補正部１２１が算出する時刻ｋにおける角速度を表わす三次の縦ベクトル。ωｂｋは、角速度センサ１１１が計測した時刻ｋにおける角速度を表わす三次の縦ベクトル。ｂｇは、第一推定部１４０が推定した角速度ωｂのバイアス誤差を表わす三次の縦ベクトル。
【００２２】
姿勢角記憶部１２４は、慣性座標系における移動体８００の姿勢角ｑを表わすデータを記憶している。姿勢角記憶部１２４は、例えば、姿勢角ｑを表わすデータとして、コータニオン（ｑｕａｔｅｒｎｉｏｎ）表示形式による四つの実数ｑ０，ｑ１，ｑ２，ｑ３を表わすデータを記憶する。
姿勢角記憶部１２４は、あらかじめ、例えば、移動体８００が水平状態で静止しているときに、図示していないコンパスなどの方位センサが計測した移動体８００の向きに基づいて算出した姿勢角を用いて、記憶した姿勢角ｑを表わすデータを初期化しておく。
【００２３】
速度記憶部１２９は、慣性座標系における移動体８００の速度ｖを表わすデータを記憶している。速度記憶部１２９は、例えば、速度ｖを表わすデータとして、慣性座標系におけるｘｙｚ三軸それぞれの方向の速度を表わす三つの実数ｖｘ，ｖｙ，ｖｚを表わすデータを記憶する。
速度記憶部１２９は、あらかじめ、例えば、移動体８００は静止しているときに求めた移動体８００の速度（すなわち「０」）を用いて、記憶した速度ｖを表わすデータを初期化しておく。
【００２４】
姿勢角記憶部１２４および速度記憶部１２９が記憶した姿勢角ｑおよび速度ｖは、その後、計測のたびに更新される。以下、時刻ｔｋにおける計測により更新された姿勢角ｑおよび速度ｖを、それぞれ「ｑｋ」「ｖｋ」と表記する。
【００２５】
姿勢角外挿計算部１２２は、姿勢角記憶部１２４が記憶した慣性座標系における姿勢角ｑを表わすデータと、角速度誤差補正部１２１が出力した補正した移動体座標系における角速度ωｂ＋を表わすデータとを入力する。姿勢角外挿計算部１２２は、入力したデータが表わす姿勢角ｑと角速度ωｂ＋とに基づいて、外挿計算をし、慣性座標系における新しい姿勢角ｑｎを算出する。姿勢角外挿計算部１２２は、算出した姿勢角ｑｎを表わすデータを出力する。
姿勢角外挿計算部１２２は、例えば、次の計算式を用いて、新しい姿勢角ｑｎを算出する。
【数１２】
ここで、ｑｎｋは、姿勢角外挿計算部１２２が算出する時刻ｔｋにおける姿勢角のコータニオン表示を成分とする四次の縦ベクトル。ｑｋ−１は、姿勢角記憶部１２４が記憶した時刻ｔｋ−１における姿勢角のコータニオン表示を成分とする四次の縦ベクトル。Ｔｓは、角速度センサ１１１が角速度ωｂを計測した周期を表わすスカラー値であり、Ｔｓ＝ｔｋ−ｔｋ−１。Ωｂｋは、四行四列の正方行列。ωＸ，ωＹ，ωＺは、角速度誤差補正部１２１が補正した時刻ｔｋにおける角速度ωｂ＋ｋのＸＹＺ三軸それぞれを中心すとする成分。
【００２６】
姿勢角誤差補正部１２３は、姿勢角外挿計算部１２２が出力した姿勢角ｑｎを表わすデータと、第一推定部１４０が出力した姿勢角ｑｎの誤差εを表わすデータとを入力する。姿勢角誤差補正部１２３は、入力したデータが表わす姿勢角ｑｎと、誤差εとに基づいて、姿勢角ｑｎの誤差を補正して、補正した姿勢角ｑｎ＋を算出する。姿勢角誤差補正部１２３は、補正した姿勢角ｑｎ＋を表わすデータを出力する。
姿勢角誤差補正部１２３は、例えば、次の計算式を用いて、補正した姿勢角ｑｎ＋を算出する。
【数１３】
ここで、ｑｎ＋ｋは、姿勢角誤差補正部１２３が算出する時刻ｔｋにおける補正した姿勢角のコータニオン表示を成分とする四次の縦ベクトル。ｑｎｋは、姿勢角外挿計算部１２２が算出した時刻ｔｋにおける姿勢角のコータニオン表示を成分とする四次の縦ベクトル。○で囲まれた×は、コータニオンの乗算。δｑは、コータニオンを表わす四次の縦ベクトル。εは、第一推定部１４０が算出した姿勢角の誤差を表わす三次の縦ベクトル。‖ε‖は、ベクトルεのノルムであるスカラー値。
【００２７】
姿勢角記憶部１２４は、姿勢角誤差補正部１２３が出力した姿勢角ｑｎ＋を表わすデータを入力する。姿勢角記憶部１２４は、入力したデータを、姿勢角ｑを表わすデータとして記憶する。姿勢角記憶部１２４が記憶した姿勢角ｑを表わすデータは、姿勢角出力部１９１が入力して、外部に出力する。
【００２８】
方向余弦行列計算部１２５は、姿勢角誤差補正部１２３が出力した補正した姿勢角ｑｎ＋を表わすデータを入力する。方向余弦行列計算部１２５は、入力したデータが表わす姿勢角ｑｎ＋に基づいて、方向余弦行列Ｃｎｂを算出する。方向余弦行列Ｃｎｂは、移動体座標系から慣性座標系への座標変換を表わす行列である。方向余弦行列計算部１２５は、算出した方向余弦行列Ｃｎｂを表わすデータを出力する。方向余弦行列計算部１２５が出力した方向余弦行列Ｃｎｂを表わすデータは、第一推定部１４０が入力し、誤差の推定に使用する。
方向余弦行列計算部１２５は、例えば、次の計算式を用いて、方向余弦行列Ｃｎｂを算出する。
【数１４】
ここで、Ｃｎｂは、方向余弦行列計算部１２５が算出する時刻ｔｋにおける方向余弦行列である三行三列の正方行列。ｑ０，ｑ１，ｑ２，ｑ３は、姿勢角誤差補正部１２３が補正した時刻ｔｋにおける姿勢角ｑｎ＋のコータニオン表示における各成分。
【００２９】
速度増分計算部１２６は、加速度センサ１１２が出力した移動体座標系における加速度ａｂを表わすデータと、方向余弦行列計算部１２５が出力した方向余弦行列Ｃｎｂを表わすデータとを入力する。速度増分計算部１２６は、入力したデータが表わす加速度ａｂと方向余弦行列Ｃｎｂとに基づいて、移動体座標系における加速度ａｂを慣性座標系における加速度に変換し、変換した加速度に基づいて、慣性座標系における移動体８００の速度の増分Δｖｎを算出する。速度増分計算部１２６は、算出した速度の増分Δｖｎを表わすデータを出力する。速度増分計算部１２６が出力した速度の増分Δｖｎを表わすデータは、第一推定部１４０が入力し、誤差の推定に使用する。
速度増分計算部１２６は、例えば、次の計算式により、速度の増分Δｖｎを算出する。
【数１５】
ここで、Δｖｎｋは、速度増分計算部１２６が算出する時刻ｔｋ−１から時刻ｔｋまでの間の速度の増分を表わす三次の縦ベクトル。Ｔｓは、加速度センサ１１２が加速度を計測した周期を表わすスカラー値で、数１２におけるＴｓと同じく、Ｔｓ＝ｔｋ−ｔｋ−１。Ｃｎｂは、方向余弦行列計算部１２５が算出した方向余弦行列である三行三列の正方行列。ａｂｋは、加速度センサ１１２が計測した時刻ｔｋにおける加速度を表わす三次の縦ベクトル。ｇｎは、慣性座標系における重力加速度を表わす三次の縦ベクトル。
【００３０】
速度外挿計算部１２７は、速度記憶部１２９が記憶した慣性座標系における速度ｖを表わすデータと、速度増分計算部１２６が出力した慣性座標系における速度の増分Δｖｎを表わすデータとを入力する。速度外挿計算部１２７は、入力したデータが表わす速度ｖと速度の増分Δｖｎとに基づいて、外挿計算をし、慣性座標系における新しい速度ｖｎを算出する。速度外挿計算部１２７は、算出した速度ｖｎを表わすデータを出力する。速度外挿計算部１２７が出力した速度ｖｎを表わすデータは、速度減算部１３０が入力し、速度の差ｄｖの算出に使用する。
速度外挿計算部１２７は、例えば、次の計算式を用いて、速度ｖｎを算出する。
【数１６】
ここで、ｖｎｋは、速度外挿計算部１２７が算出する時刻ｔｋにおける速度を表わす三次の縦ベクトル。ｖｋ−１は、速度記憶部１２９が記憶した時刻ｔｋ−１における速度を表わす三次の縦ベクトル。Δｖｎｋは、速度増分計算部１２６が算出した時刻ｔｋ−１から時刻ｔｋまでの間の速度の増分を表わす三次の縦ベクトル。
【００３１】
速度誤差補正部１２８は、速度外挿計算部１２７が出力した慣性座標系における速度ｖｎを表わすデータと、第二推定部１５０が出力した速度の誤差δｖを表わすデータとを入力する。速度誤差補正部１２８は、入力したデータが表わす速度ｖｎと速度の誤差δｖとに基づいて、速度ｖｎの誤差を補正して、補正した速度ｖｎ＋を算出する。速度誤差補正部１２８は、算出した速度ｖｎ＋を表わすデータを出力する。
速度誤差補正部１２８は、例えば、次の計算式を用いて、速度ｖｎ＋を算出する。
【数１７】
ここで、ｖｎ＋ｋは、速度誤差補正部１２８が算出する時刻ｔｋにおける補正した速度を表わす三次の縦ベクトル。ｖｎｋは、速度外挿計算部１２７が算出した時刻ｔｋにおける速度を表わす三次の縦ベクトル。δｖｋは、第二推定部１５０が推定した時刻ｔｋにおける速度の誤差を表わす三次の縦ベクトル。
【００３２】
なお、ＧＰＳ受信機１１３が移動体８００の位置・速度を計測していない場合、速度誤差補正部１２８は、速度誤差の補正をせず、入力したデータが表わす速度ｖｎを、そのまま速度ｖｎ＋とする。
【００３３】
速度記憶部１２９は、速度誤差補正部１２８が出力した慣性座標系における速度ｖｎ＋を表わすデータを入力する。速度記憶部１２９は、入力したデータを、速度ｖを表わすデータとして記憶する。速度記憶部１２９が記憶した速度ｖを表わすデータは、速度出力部１９２が入力して、外部に出力する。
【００３４】
図３は、この実施の形態における第一推定部１４０の機能ブロックの構成の一例を示す詳細ブロック図である。
第一推定部１４０は、カルマンフィルタを用いて、角速度センサ１１１が計測する角速度ωｂのバイアス誤差ｂｇと、慣性航法部１２０が推定する姿勢角の誤差εとを推定する。
第一推定部１４０が使用するカルマンフィルタは、例えば、次の式で表わされる六次の縦ベクトルｘを状態量とし、三次の縦ベクトルΔｚを観測量とする。
【数１８】
【００３５】
第一推定部１４０は、観測行列計算部１４１、推移行列計算部１４２、誤差共分散外挿計算部１４３、カルマンゲイン計算部１４４、誤差共分散観測更新計算部１４５、誤差共分散記憶部１４６、状態量外挿計算部１４７、状態量観測更新計算部１４８、状態量記憶部１４９を有する。
【００３６】
観測行列計算部１４１は、慣性航法部１２０の速度増分計算部１２６が出力した速度の増分Δｖｎを表わすデータを入力する。観測行列計算部１４１は、入力したデータが表わす速度の増分Δｖｎに基づいて、カルマンフィルタで使用する観測行列Ｈを算出する。観測行列計算部１４１は、算出した観測行列Ｈを表わすデータを出力する。
観測行列計算部１４１は、例えば、次の計算式を用いて、観測行列Ｈを算出する。
【数１９】
ここで、Ｈは、観測行列計算部１４１が算出する観測行列である三行六列の行列。０３×３は、三行三列の零行列。Ωｖは、三行三列の正方行列。Δｖｎｘ，Δｖｎｙ，Δｖｎｚは、速度増分計算部１２６が算出した時刻ｔｋ−１から時刻ｔｋまでの間の速度の増分Δｖｎｋのｘｙｚ三軸それぞれの方向成分。
【００３７】
なお、観測行列計算部１４１が計算する観測行列Ｈは、ＧＰＳ受信機１１３が移動体８００の位置ｐｇや速度ｖｇを計測し、速度減算部１３０が速度の差ｄｖを算出した場合のみ使用される。このため、観測行列計算部１４１は、ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したか否かを判定し、計測したと判定した場合は観測行列Ｈを算出し、計測していないと判定した場合は観測行列Ｈを算出しない構成としてもよい。
【００３８】
推移行列計算部１４２は、慣性航法部１２０の方向余弦行列計算部１２５が出力した方向余弦行列Ｃｎｂを表わすデータを入力する。推移行列計算部１４２は、入力したデータが表わす方向余弦行列Ｃｎｂに基づいて、カルマンフィルタで使用する推移行列Φを算出する。推移行列計算部１４２は、算出した推移行列Φを表わすデータを出力する。
推移行列計算部１４２は、例えば、次の計算式を用いて、推移行列Φを算出する。
【数２０】
ここで、Φは、推移行列計算部１４２が算出する推移行列である六行六列の正方行列。Ｉ３×３は、三行三列の単位行列。０３×３は、三行三列の零行列。Ｔｓは、角速度センサ１１１及び加速度センサ１１２が角速度や加速度を計測する周期を表わすスカラー値で、数１２や数１５におけるＴｓと同じく、Ｔｓ＝ｔｋ−ｔｋ−１。
【００３９】
誤差共分散記憶部１４６は、カルマンフィルタにおける誤差共分散行列Ｐを表わすデータを記憶している。誤差共分散記憶部１４６は、例えば、六行六列の行列である誤差共分散行列Ｐを表わすデータとして、６×６＝３６個の実数を表わすデータを記憶する。誤差共分散記憶部１４６は、例えば、あらかじめ、誤差共分散行列Ｐの初期値として設定した所定の行列を用いて、記憶した誤差共分散行列Ｐを表わすデータを初期化しておく。
【００４０】
状態量記憶部１４９は、カルマンフィルタにおける状態量ｘを表わすデータを記憶している。状態量記憶部１４９は、例えば、六次の縦ベクトルである状態量ｘを表わすデータとして、六個の実数を表わすデータを記憶する。状態量記憶部１４９は、例えば、あらかじめ、状態量ｘの初期値として設定した所定のベクトルを用いて、記憶した状態量ｘを表わすデータを初期化しておく。
【００４１】
誤差共分散記憶部１４６および状態量記憶部１４９が記憶した誤差共分散行列Ｐおよび状態量ｘは、その後、計測のたびに更新される。以下、時刻ｔｋにおける計測により更新された誤差共分散行列Ｐおよび状態量ｘを、それぞれ「Ｐｋ」「ｘｋ」と表記する。
【００４２】
誤差共分散外挿計算部１４３は、推移行列計算部１４２が出力した推移行列Φを表わすデータと、誤差共分散記憶部１４６が記憶した誤差共分散行列Ｐを表わすデータとを入力する。誤差共分散外挿計算部１４３は、入力したデータが表わす推移行列Φと、誤差共分散行列Ｐとに基づいて、誤差共分散行列の外挿計算をして、外挿した誤差共分散行列Ｐ−を算出する。誤差共分散外挿計算部１４３は、算出した誤差共分散行列Ｐ−を表わすデータを出力する。
誤差共分散外挿計算部１４３は、例えば、次の計算式を用いて、誤差共分散行列Ｐ−を算出する。
【数２１】
ここで、Ｐ−ｋは、誤差共分散外挿計算部１４３が算出する時刻ｔｋにおける外挿した誤差共分散行列である六行六列の正方行列。Ｐｋ−１は、誤差共分散記憶部１４６が記憶した時刻ｔｋ−１における誤差共分散行列である六行六列の正方行列。Φは、推移行列計算部１４２が算出した推移行列である六行六列の正方行列。ΦＴは、行列Φの転置行列である六行六列の行列。Ｑは、あらかじめ設定されたプロセスノイズ行列である六行六列の正方行列。
誤差共分散外挿計算部１４３は、プロセスノイズ行列Ｑとして、あらかじめ、設定された値を記憶している。
【００４３】
状態量外挿計算部１４７は、推移行列計算部１４２が出力した推移行列Φを表わすデータと、状態量記憶部１４９が記憶した状態量ｘを表わすデータとを入力する。状態量外挿計算部１４７は、入力したデータが表わす推移行列Φと状態量ｘとに基づいて、状態量の外挿計算をして、外挿した状態量ｘ−を算出する。状態量外挿計算部１４７は、算出した外挿した状態量ｘ−を表わすデータを出力する。
状態量外挿計算部１４７は、例えば、次の計算式を用いて、外挿した状態量ｘ−を算出する。
【数２２】
ここで、ｘ−ｋは、状態量外挿計算部１４７が算出する時刻ｔｋにおける外挿した状態量である六次の縦ベクトル。Φは、推移行列計算部１４２が算出した推移行列である六行六列の正方行列。ｘｋ−１は、状態量記憶部１４９が記憶した時刻ｔｋ−１における状態量である六次の縦ベクトル。
【００４４】
カルマンゲイン計算部１４４は、観測行列計算部１４１が出力した観測行列Ｈを表わすデータと、誤差共分散外挿計算部１４３が出力した誤差共分散行列Ｐ−を表わすデータとを入力する。カルマンゲイン計算部１４４は、入力したデータが表わす観測行列Ｈと誤差共分散行列Ｐ−とに基づいて、カルマンフィルタにおけるカルマンゲインＫを算出する。カルマンゲイン計算部１４４は、算出したカルマンゲインＫを表わすデータを出力する。
カルマンゲイン計算部１４４は、例えば、次の計算式を用いて、カルマンゲインＫを算出する。
【数２３】
ここで、Ｋは、カルマンゲイン計算部１４４が算出する時刻ｔｋにおけるカルマンゲインである六行三列の行列。Ｐ−ｋは、誤差共分散外挿計算部１４３が算出した時刻ｔｋにおける外挿した誤差共分散行列である六行六列の正方行列。Ｈは、観測行列計算部１４１が算出した時刻ｔｋにおける観測行列である三行六列の行列。ＨＴは、観測行列Ｈの転置行列である六行三列の行列。Ｒは、あらかじめ設定された観測誤差の共分散行列である三行三列の行列。（〜）−１は、逆行列。
カルマンゲイン計算部１４４は、観測誤差の共分散行列Ｒとして、あらかじめ、設定された値を記憶している。
【００４５】
なお、カルマンゲイン計算部１４４が計算するカルマンゲインＫは、ＧＰＳ受信機１１３が移動体８００の位置ｐｇや速度ｖｇを計測し、速度減算部１３０が速度の差ｄｖを算出した場合のみ使用される。このため、カルマンゲイン計算部１４４は、ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したか否かを判定し、計測したと判定した場合はカルマンゲインＫを算出し、計測していないと判定した場合はカルマンゲインＫを算出しない構成としてもよい。
【００４６】
誤差共分散観測更新計算部１４５は、観測行列計算部１４１が出力した観測行列Ｈを表わすデータと、誤差共分散外挿計算部１４３が出力した外挿した誤差共分散行列Ｐ−を表わすデータと、カルマンゲイン計算部１４４が出力したカルマンゲインＫを表わすデータとを入力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したと判定した場合、誤差共分散観測更新計算部１４５は、入力したデータが表わす観測行列Ｈと外挿した誤差共分散行列Ｐ−とカルマンゲインＫとに基づいて、誤差共分散行列を観測更新し、観測更新した誤差共分散行列Ｐ＋を算出する。誤差共分散観測更新計算部１４５は、算出した観測更新した誤差共分散行列Ｐ＋を表わすデータを出力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測していないと判定した場合、誤差共分散観測更新計算部１４５は、入力したデータが表わす外挿した誤差共分散行列Ｐ−をそのまま誤差共分散行列Ｐ＋とし、誤差共分散行列Ｐ＋を表わすデータを出力する。
誤差共分散観測更新計算部１４５は、例えば、次の計算式を用いて、観測更新した誤差共分散行列Ｐ＋を算出する。
【数２４】
ここで、Ｐ＋ｋは、誤差共分散観測更新計算部１４５が算出する時刻ｔｋにおける観測更新した誤差共分散行列である六行六列の行列。Ｉ６×６は、六行六列の単位行列。Ｋは、カルマンゲイン計算部１４４が算出したカルマンゲインである六行三列の行列。Ｈは、観測行列計算部１４１が算出した観測行列である三行六列の行列。Ｐ−ｋは、誤差共分散外挿計算部１４３が算出した外挿した誤差共分散行列である六行六列の行列。
【００４７】
誤差共分散記憶部１４６は、誤差共分散観測更新計算部１４５が出力した誤差共分散行列Ｐ＋を表わすデータを入力する。誤差共分散記憶部１４６は、入力したデータを、誤差共分散行列Ｐを表わすデータとして記憶する。
【００４８】
状態量観測更新計算部１４８は、状態量外挿計算部１４７が出力した外挿した状態量ｘ−を表わすデータと、カルマンゲイン計算部１４４が出力したカルマンゲインＫを表わすデータと、速度減算部１３０が出力した速度の差ｄｖを表わすデータとを入力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測したと判定した場合、状態量観測更新計算部１４８は、入力したデータが表わす外挿した状態量ｘ−とカルマンゲインＫと速度の差ｄｖとに基づいて、状態量を観測更新し、観測更新した状態量ｘ＋を算出する。状態量観測更新計算部１４８は、算出した観測更新した状態量ｘ＋を表わすデータを出力する。
ＧＰＳ受信機１１３が時刻ｔｋにおける移動体８００の位置ｐｇや速度ｖｇを計測していないと判定した場合、状態量観測更新計算部１４８は、入力したデータが表わす外挿した状態量ｘ−をそのまま状態量ｘ＋とし、状態量ｘ＋を表わすデータを出力する。
状態量観測更新計算部１４８は、例えば、次の計算式を用いて、観測更新した状態量ｘ＋を算出する。
【数２５】
ここで、ｘ＋ｋは、状態量観測更新計算部１４８が算出する観測更新した状態量である六次の縦ベクトル。ｘ−ｋは、状態量外挿計算部１４７が算出した外挿した状態量である六次の縦ベクトル。Ｋは、カルマンゲイン計算部１４４が算出したカルマンゲインである六行三列の行列。Δｚは、観測量である三次の縦ベクトル。ｄｖは、速度減算部１３０が算出した速度の差である三次の縦ベクトル。
【００４９】
状態量記憶部１４９は、状態量観測更新計算部１４８が出力した状態量ｘ＋を表わすデータを入力する。状態量記憶部１４９は、入力したデータを、状態量ｘを表わすデータとして記憶する。状態量記憶部１４９が記憶した状態量ｘ（角速度ωｂのバイアス誤差ｂｇおよび姿勢角ｑの誤差ε）を表わすデータは、慣性航法部１２０が入力し、誤差の補正に使用する。
【００５０】
図４は、この実施の形態における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図である。
第二推定部１５０は、ＧＰＳ受信機１１３が推定したＧＰＳ受信機１１３の計測精度に基づいて、慣性航法部１２０が推定した速度ｖの誤差δｖを推定する。
第二推定部１５０は、ゲイン算出部１５１、速度誤差推定部１５２を有する。
【００５１】
ゲイン算出部１５１は、ＧＰＳ受信機１１３が出力した計測の精度を表わすデータを入力する。ゲイン算出部１５１は、入力したデータが表わす精度に基づいて、速度誤差のゲインＫｖを算出する。ゲイン算出部１５１は、算出したゲインＫｖを表わすデータを出力する。
ゲイン算出部１５１が算出するゲインＫｖは、例えば、０以上１以下の値をとるスカラー値である。この場合、例えば、ＧＰＳ受信機１１３の計測精度が高く、ＧＰＳ受信機１１３が計測した速度ｖｇが完全に信頼できる場合、ゲイン算出部１５１は、ゲインＫｖとして「１」を算出する。逆に、ＧＰＳ受信機１１３の計測精度が低く、ＧＰＳ受信機１１３が計測した速度ｖｇが全く信頼できない場合、ゲイン算出部１５１は、ゲインＫｖとして「０」を算出する。通常は、ＧＰＳ受信機１１３が計測した速度ｖｇが完全に信頼できることも全く信頼できないこともないので、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した精度に基づいて、ゲインＫＶとして「０」と「１」との値を算出する。ゲイン算出部１５１は、ＧＰＳ受信機１１３の計測精度が高いほど、算出するゲインＫｖの値を大きくする。
ゲイン算出部１５１は、例えば、あらかじめ、ＧＰＳ受信機１１３の計測精度をゲインＫｖに変換する変換テーブルを表わすデータを記憶しておく。ゲイン算出部１５１は、記憶したデータが表わす変換テーブルを参照し、入力したデータが表わす計測精度に対応するゲインＫｖを算出する。
【００５２】
なお、ゲイン算出部１５１が算出するゲインＫｖは、例えば、三行三列の行列であってもよい。例えば、ＧＰＳ受信機１１３が、水平方向の精度と垂直方向の精度というように、方向によって異なる複数の精度を算出する場合、ゲイン算出部１５１は、慣性座標系におけるｘｙｚ三軸の各軸方向によって異なるゲインを算出して、行列形式で表わされるゲインＫｖとする構成であってもよい。
【００５３】
また、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した計測の精度にかかわらず、あらかじめ設定した一定の値（例えば「０．５」）をゲインとして算出する構成であってもよい。その場合、ＧＰＳ受信機１１３は、計測の精度を推定する機能のないものであってもよい。
【００５４】
速度誤差推定部１５２は、速度減算部１３０が出力した速度の差ｄｖを表わすデータと、ゲイン算出部１５１が出力したゲインＫｖを表わすデータとを入力する。速度誤差推定部１５２は、入力したデータが表わす速度の差ｄｖとゲインＫｖとに基づいて、慣性航法部１２０が推定した速度の誤差δｖを算出する。速度誤差推定部１５２は、算出した速度の誤差δｖを表わすデータを出力する。速度誤差推定部１５２が出力した速度の誤差δｖを表わすデータは、慣性航法部１２０が入力し、誤差の補正に使用する。
速度誤差推定部１５２は、例えば、次の計算式を用いて、速度の誤差δｖを算出する。
【数２６】
ここで、δｖは、速度誤差推定部１５２が推定する速度の誤差を表わす三次の縦ベクトル。Ｋｖは、ゲイン算出部１５１が算出した速度誤差のゲインを表わすスカラー値もしくは三行三列の行列。ｄｖは、速度減算部１３０が算出した速度の差を表わす三次の縦ベクトル。
【００５５】
次に、動作について説明する。
【００５６】
図５は、この実施の形態における航法装置１００が移動体８００の速度・姿勢角を推定する航法処理の全体的な流れの一例を示すフローチャート図である。
航法処理は、初期化処理Ｓ５１０、外挿計算処理Ｓ５２０、ＧＰＳ計測判定工程Ｓ５４０、観測更新処理Ｓ５５０を有する。
【００５７】
初期化処理Ｓ５１０において、航法装置１００は、記憶したデータを初期化する。
外挿計算処理Ｓ５２０において、航法装置１００は、角速度センサ１１１や加速度センサ１１２が計測した角速度・加速度に基づいて、推定した速度・姿勢角などを外挿計算する。
【００５８】
ＧＰＳ計測判定工程Ｓ５４０において、航法装置１００は、ＧＰＳ受信機１１３が移動体８００の位置・速度を計測したか否かを判定する。
ＧＰＳ受信機１１３が移動体８００の位置・速度を計測したと判定した場合、航法装置１００は、観測更新処理Ｓ５５０へ進む。
ＧＰＳ受信機１１３が移動体８００の位置・速度を計測していないと判定した場合、航法装置１００は、外挿計算処理Ｓ５２０に戻る。
【００５９】
観測更新処理Ｓ５５０において、航法装置１００は、ＧＰＳ受信機１１３が計測した移動体８００の速度に基づいて、推定した速度・姿勢角などを観測更新する。その後、航法装置１００は、外挿計算処理Ｓ５２０に戻る。
【００６０】
図６は、この実施の形態における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図である。
初期化処理Ｓ５１０は、姿勢角初期化工程Ｓ５１１、速度初期化工程Ｓ５１２、誤差共分散初期化工程Ｓ５１４、状態量初期化工程Ｓ５１５を有する。
【００６１】
姿勢角初期化工程Ｓ５１１において、姿勢角記憶部１２４は、記憶した姿勢角ｑを初期化する。
速度初期化工程Ｓ５１２において、速度記憶部１２９は、記憶した速度ｖを初期化する。
誤差共分散初期化工程Ｓ５１４において、誤差共分散記憶部１４６は、記憶した誤差共分散行列Ｐを初期化する。
状態量初期化工程Ｓ５１５において、状態量記憶部１４９は、記憶した状態量ｘを初期化する。
【００６２】
図７は、この実施の形態における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図である。
外挿計算処理Ｓ５２０は、角速度誤差補正工程Ｓ５２１、姿勢角外挿計算工程Ｓ５２２、姿勢角誤差補正工程Ｓ５２３、方向余弦行列計算工程Ｓ５２４、速度増分計算工程Ｓ５２５、速度外挿計算工程Ｓ５２６、推移行列計算工程Ｓ５３１、誤差共分散外挿計算工程Ｓ５３２、状態量外挿計算工程Ｓ５３３を有する。
【００６３】
角速度誤差補正工程Ｓ５２１において、角速度誤差補正部１２１は、状態量記憶部１４９が記憶した状態量ｘのうち角速度のバイアス誤差ｂｇに基づいて、角速度センサ１１１が計測した角速度ωｂを補正し、補正した角速度ωｂ＋を算出する。
姿勢角外挿計算工程Ｓ５２２において、姿勢角外挿計算部１２２は、角速度誤差補正工程Ｓ５２１で角速度誤差補正部１２１が補正した角速度ωｂ＋に基づいて、姿勢角記憶部１２４が記憶した姿勢角ｑを外挿し、外挿した姿勢角ｑｎを算出する。
姿勢角誤差補正工程Ｓ５２３において、姿勢角誤差補正部１２３は、状態量記憶部１４９が記憶した状態量ｘのうち姿勢角の誤差εに基づいて、姿勢角外挿計算工程Ｓ５２２で姿勢角外挿計算部１２２が外挿した姿勢角ｑｎを補正し、補正した姿勢角ｑｎ＋を算出する。姿勢角記憶部１２４は、姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋を、新しい姿勢角ｑとして記憶する。
方向余弦行列計算工程Ｓ５２４において、方向余弦行列計算部１２５は、姿勢角誤差補正工程Ｓ５２３で姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋に基づいて、方向余弦行列Ｃｎｂを算出する。
速度増分計算工程Ｓ５２５において、速度増分計算部１２６は、方向余弦行列計算工程Ｓ５２４で方向余弦行列計算部１２５が算出した方向余弦行列Ｃｎｂに基づいて、加速度センサ１１２が計測した移動体座標系における加速度ａｂを、慣性座標系における加速度に座標変換し、座標変換した慣性座標系における加速度に基づいて、速度の増分Δｖｎを算出する。
速度外挿計算工程Ｓ５２６において、速度外挿計算部１２７は、速度増分計算工程Ｓ５２５で速度増分計算部１２６が算出した速度の増分Δｖｎに基づいて、速度記憶部１２９が記憶した速度ｖを外挿し、外挿した速度ｖｎを算出する。速度記憶部１２９は、速度外挿計算部１２７が外挿した速度ｖｎを、暫定的な速度ｖとして記憶する。
【００６４】
推移行列計算工程Ｓ５３１において、推移行列計算部１４２は、方向余弦行列計算工程Ｓ５２４で方向余弦行列計算部１２５が算出した方向余弦行列Ｃｎｂに基づいて、推移行列Φを算出する。
誤差共分散外挿計算工程Ｓ５３２において、誤差共分散外挿計算部１４３は、推移行列計算工程Ｓ５３１で推移行列計算部１４２が算出した推移行列Φに基づいて、誤差共分散記憶部１４６が記憶した誤差共分散行列Ｐを外挿し、外挿した誤差共分散行列Ｐ−を算出する。ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、誤差共分散外挿計算部１４３が算出した誤差共分散行列Ｐ−が、そのまま誤差共分散記憶部１４６が記憶する誤差共分散行列Ｐとなるので、誤差共分散記憶部１４６は、誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−を、暫定的に、誤差共分散行列Ｐとして記憶する。
状態量外挿計算工程Ｓ５３３において、状態量外挿計算部１４７は、推移行列計算工程Ｓ５３１で推移行列計算部１４２が算出した推移行列Φに基づいて、状態量記憶部１４９が記憶した状態量ｘを外挿し、外挿した状態量ｘ−を算出する。ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、状態量外挿計算部１４７が算出した外挿した状態量ｘ−が、そのまま状態量記憶部１４９が記憶する状態量ｘとなるので、状態量記憶部１４９は、状態量外挿計算部１４７が外挿した状態量ｘ−を、暫定的に、状態量ｘとして記憶する。
【００６５】
図８は、この実施の形態における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図である。
観測更新処理Ｓ５５０は、速度減算工程Ｓ５５１、観測行列計算工程Ｓ５５２、カルマンゲイン計算工程Ｓ５５３、誤差共分散観測更新計算工程Ｓ５５４、状態量観測更新計算工程Ｓ５５５、ゲイン算出工程Ｓ５６１、速度誤差推定工程Ｓ５６２、速度誤差補正工程Ｓ５６３を有する。
【００６６】
速度減算工程Ｓ５５１において、速度減算部１３０は、ＧＰＳ受信機１１３が計測した速度ｖｇと、速度外挿計算工程Ｓ５２６で速度外挿計算部１２７が外挿した速度ｖｎとに基づいて、速度の差ｄｖを算出する。
観測行列計算工程Ｓ５５２において、観測行列計算部１４１は、速度増分計算工程Ｓ５２５で速度増分計算部１２６が算出した速度の増分Δｖｎに基づいて、観測行列Ｈを算出する。
カルマンゲイン計算工程Ｓ５５３において、カルマンゲイン計算部１４４は、誤差共分散外挿計算工程Ｓ５３２で誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−と、観測行列計算工程Ｓ５５２で観測行列計算部１４１が算出した観測行列Ｈとに基づいて、カルマンゲインＫを算出する。
誤差共分散観測更新計算工程Ｓ５５４において、誤差共分散観測更新計算部１４５は、観測行列計算工程Ｓ５５２で観測行列計算部１４１が算出した観測行列Ｈと、カルマンゲイン計算工程Ｓ５５３でカルマンゲイン計算部１４４が計算したカルマンゲインＫとに基づいて、誤差共分散外挿計算工程Ｓ５３２で誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−を観測更新し、観測更新した誤差共分散行列Ｐ＋を算出する。誤差共分散記憶部１４６は、誤差共分散観測更新計算部１４５が観測更新した誤差共分散行列Ｐ＋を、誤差共分散行列Ｐとして記憶する。
状態量観測更新計算工程Ｓ５５５において、状態量観測更新計算部１４８は、カルマンゲイン計算工程Ｓ５５３でカルマンゲイン計算部１４４が計算したカルマンゲインＫに基づいて、状態量外挿計算工程Ｓ５３３で状態量外挿計算部１４７が外挿した状態量ｘ−を観測更新し、観測更新した状態量ｘ＋を算出する。状態量記憶部１４９は、状態量観測更新計算部１４８が観測更新した状態量ｘ＋を、状態量ｘとして記憶する。
なお、ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、観測更新処理Ｓ５５０を実行しないので、誤差共分散記憶部１４６は、誤差共分散外挿計算工程Ｓ５３２で暫定的に記憶した誤差共分散行列Ｐ（すなわち、誤差共分散外挿計算部１４３が外挿した誤差共分散行列Ｐ−）をそのまま記憶し続け、状態量記憶部１４９は、状態量外挿計算工程Ｓ５３３で暫定的に記憶した状態量ｘ（すなわち、状態量外挿計算部１４７が外挿した状態量ｘ−）をそのまま記憶し続けることとなる。
【００６７】
ゲイン算出工程Ｓ５６１において、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した計測の精度に基づいて、速度誤差のゲインＫｖを算出する。
速度誤差推定工程Ｓ５６２において、速度誤差推定部１５２は、速度減算工程Ｓ５５１で速度減算部１３０が算出した速度の差ｄｖと、ゲイン算出工程Ｓ５６１でゲイン算出部１５１が算出した速度誤差のゲインＫｖとに基づいて、速度の誤差δｖを算出する。
速度誤差補正工程Ｓ５６３において、速度誤差補正部１２８は、速度誤差推定工程Ｓ５６２で速度誤差推定部１５２が算出した速度の誤差δｖに基づいて、速度外挿計算工程Ｓ５２６で速度外挿計算部１２７が外挿した速度ｖｎを補正し、補正した速度ｖｎ＋を算出する。速度記憶部１２９は、速度誤差補正部１２８が補正した速度ｖｎ＋を、速度ｖとして記憶する。
【００６８】
慣性航法部１２０が推定した姿勢角ｑが誤差εを含んでいる場合、その誤差εは、方向余弦行列Ｃｎｂを介して、速度増分計算部１２６が座標変換した加速度に伝播し、速度増分計算部１２６が算出する速度の増分Δｖｎに反映する。誤差εが反映された速度の増分Δｖｎに基づいて観測行列計算部１４１が算出した観測行列Ｈを用いて、誤差εと、観測量Δｚ（速度の差ｄｖ）とを結びつけることにより、カルマンフィルタを用いて、状態量ｘ（バイアス誤差ｂｇ及び誤差ε）を推定することができる。
【００６９】
ここで、加速度センサ１１２が計測する加速度ａｂの誤差が大きい場合には、その誤差も、速度増分計算部１２６が算出する速度の増分Δｖｎに反映する。そのため、例えば、位置の誤差、速度の誤差、姿勢角の誤差、角速度センサの誤差、加速度センサの誤差を状態量とし、角速度センサや加速度センサに基づいて推定した位置及び速度と、ＧＰＳ受信機が計測した位置及び速度との差を観測量とする必要が生じる。
これに対し、加速度センサ１１２が計測する加速度ａｂの誤差が無視できるほど小さい場合には、バイアス誤差ｂｇと誤差εとのみを状態量としても、精度よく推定をすることができる。
【００７０】
航法装置１００の製造コストを低く抑えるため、角速度センサ１１１や加速度センサ１１２として、安価なセンサを使用する場合、角速度センサは、バイアス誤差が比較的大きいが、加速度センサには、バイアス誤差が比較的小さく、適切な補償をすることにより、バイアス誤差を無視できるものが存在する。
【００７１】
カルマンフィルタを用いた推定において、状態量ベクトルの次数は、推定に必要な計算量に大きな影響を与える。いま、状態量ベクトルの次数をｎ、プロセスノイズ行列の次数をｍ、観測量ベクトルの次数をｒとすると、カルマンフィルタによる１回の観測更新における乗算の回数は、以下の式で与えられる。
【数２７】
なお、加算の計算負荷は、乗算の計算負荷に比べてはるかに小さいので、推定に必要な計算量は、乗算の回数により見積もることができる。
【００７２】
この実施の形態における航法装置１００において、ｎ＝ｍ＝６、ｒ＝３であるから、Ｓｍ＝７８６である。
比較例として、位置の誤差（三次）、速度の誤差（三次）、姿勢角の誤差ε（三次）、角速度センサの誤差ｂｇ（三次）、加速度センサの誤差ｂａ（三次）を状態量（１５次）とし、位置の差（三次）、速度の差（三次）を観測量（六次）とした場合を考えると、ｎ＝ｍ＝１５、ｒ＝６であるから、Ｓｍ＝９８５５である。
このように、この実施の形態における航法装置１００は、比較例に対して、乗算の回数を１０分の１以下に抑えることができる。
【００７３】
ＧＰＳ受信機１１３が移動体８００の位置や速度を計測する頻度は、１秒間に１〜４回程度であるから、実時間で航法処理をするには、１秒間に１〜４回の観測更新処理をする必要がある。
【００７４】
この実施の形態における航法装置１００は、１回の処理に必要な計算量が少ないので、比較的処理能力の低いマイクロコンピュータなどを使用して、慣性航法部１２０や第一推定部１４０などを実現しても、実時間で処理を行うことができる。
このため、航法装置１００の製造コストを抑えることができる。
【００７５】
この実施の形態における航法装置１００の構成により、推定の精度が劣化しないことを確認するため、以下のような実験を行った。
【００７６】
（１）シミュレーションによる実験
実機での実験に先立ち、以下の条件のもと、シミュレーションによる模擬実験を行った。
角速度センサ１１１の誤差モデル：ノイズ誤差０．０５４度／秒／√Ｈｚ、バイアス誤差０．１９度／秒／√ｈ、スケールファクタ１．０％、初期化誤差０．１％。
加速度センサ１１２の誤差モデル：ノイズ誤差４１．７μｇ／√Ｈｚ、バイアス誤差０．０２８ｍ／ｓ２／√ｈ、スケールファクタ１．０％、初期化誤差０．１％。
ＧＰＳデータ：２００７年７月２０日のデータ。平均利用可能衛星数７。
【００７７】
比較対象として、位置誤差、速度誤差、姿勢角誤差、角速度バイアス誤差、加速度バイアス誤差を状態量（１５次）とする従来の疎結合ＧＰＳ／ＩＮＳ（比較例１）、更に、状態量にクロックバイアス誤差とクロックドリフト誤差を加えた（１７次）従来の密結合ＧＰＳ／ＩＮＳ（比較例２）についても、同じ条件でシミュレーションを行った。
【００７８】
その結果、この実施の形態における航法装置１００における姿勢角の推定誤差の二乗平均平方根（ＲＭＳ）は、ロール方向で０．３１度、ピッチ方向で０．４２度、ヨー方向で１．５５度であった。
これに対し、比較例１は、ロール方向で０．３３度、ピッチ方向で０．４１度、ヨー方向で１．３４度であった。比較例２は、ロール方向で０．２８度、ピッチ方向で０．３７度、ヨー方向で１．２２度であった。
【００７９】
この模擬実験の結果によれば、この実施の形態における航法装置１００の構成による推定の精度は、従来の構成とほとんど変わらない。
【００８０】
（２）実機による実験
模擬実験により、一応の確認が取れたので、更に、以下の条件のもと、実機による実験を行った。
センサ：自動車用低価格ＭＥＭＳ慣性センサ。計測周波数６４Ｈｚ。
ＧＰＳ受信機：計測周波数４Ｈｚ。
参照データの計測：リングレーザージャイロを用いたＧＰＳ／ＩＮＳ（姿勢角の推定誤差の二乗平均平方根０．１度）。
比較対象：模擬実験と同じ。
【００８１】
実験の結果、この実施の形態における航法装置１００における姿勢角の推定誤差の二乗平均平方根は、ロール方向で０．２４度、ピッチ方向で０．１７度、ヨー方向で１．４１度であった。
これに対し、比較例１は、ロール方向で０．２５度、ピッチ方向で０．２０度、ヨー方向で１．７８度であった。比較例２は、ロール方向で０．２４度、ピッチ方向で０．２５度、ヨー方向で１．３１度であった。
【００８２】
この実機による実験の結果によれば、この実施の形態における航法装置１００の構成による推定の精度は、従来の構成とほとんど変わらない。
【００８３】
以上のように、この実施の形態における航法装置１００によれば、推定の精度を劣化させることなく、カルマンフィルタの計算量を減らすことができる。
【００８４】
この実施の形態における航法装置１００は、移動体８００に搭載される航法装置である。
上記航法装置１００は、角速度計測部（角速度センサ１１１）と、加速度計測部（加速度センサ１１２）と、速度計測部（ＧＰＳ受信機１１３）と、姿勢角外挿計算部１２２と、速度増分計算部１２６と、速度外挿計算部１２７と、速度減算部１３０と、第一推定部１４０とを有する。
上記角速度計測部（角速度センサ１１１）は、上記移動体８００を基準とした第一座標系（移動体座標系）における上記移動体８００の角速度ωｂを計測する。
上記加速度計測部（加速度センサ１１２）は、上記第一座標系における上記移動体８００に加わる加速度ａｂを計測する。
上記速度計測部（ＧＰＳ受信機１１３）は、上記第一座標系と異なる第二座標系（慣性座標系）における上記移動体８００の速度ｖｇを計測する。
上記姿勢角外挿計算部１２２は、上記角速度計測部（角速度センサ１１１）が計測した角速度ωｂに基づいて、上記第二座標系における上記移動体８００の姿勢角ｑｎを計算する。
上記速度増分計算部１２６は、上記加速度計測部（加速度センサ１１２）が計測した加速度ａｂと、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎとに基づいて、上記第二座標系における上記移動体８００の速度の増分Δｖｎを計算する。
上記速度外挿計算部１２７は、上記速度増分計算部１２６が計算した速度の増分Δｖｎに基づいて、上記第二座標系における上記移動体８００の速度ｖｎを計算する。
上記速度減算部１３０は、上記速度外挿計算部１２７が計算した速度ｖｎと、上記速度計測部（ＧＰＳ受信機１１３）が計測した速度ｖｇとの差ｄｖを計算する。
上記第一推定部１４０は、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、上記速度増分計算部１２６が計算した速度の増分Δｖｎと、上記速度減算部１３０が計算した速度の差ｄｖとに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを推定する。
【００８５】
この実施の形態における航法装置１００によれば、姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、速度増分計算部１２６が計算した速度の増分Δｖｎと、速度減算部１３０が計算した速度の差ｄｖとに基づいて、第一推定部１４０が角速度の誤差ｂｇと、姿勢角の誤差εとを推定するので、推定にかかる計算量が少なく、航法装置１００の製造コストを低く抑えることができる。
【００８６】
この実施の形態における航法装置１００は、更に、角速度誤差補正部１２１と、姿勢角誤差補正部１２３とを有する。
上記角速度誤差補正部１２１は、上記第一推定部１４０が推定した角速度の誤差ｂｇに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度ωｂを補正する。
上記姿勢角外挿計算部１２２は、上記角速度誤差補正部１２１が補正した角速度ωｂ＋に基づいて、上記第二座標系における上記移動体８００の姿勢角ｑｎを計算する。
上記姿勢角誤差補正部１２３は、上記第一推定部１４０が推定した姿勢角の誤差εに基づいて、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎを補正する。
上記速度増分計算部１２６は、上記加速度計測部（加速度センサ１１２）が計測した加速度ａｂと、上記姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋とに基づいて、上記第二座標系における上記移動体８００の速度の増分Δｖｎを計算する。
上記第一推定部１４０は、上記姿勢角誤差補正部１２３が補正した姿勢角ｑｎ＋と、上記速度増分計算部１２６が計算した速度の増分Δｖｎと、上記速度減算部１３０が計算した速度の差ｄｖとに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを推定する。
【００８７】
この実施の形態における航法装置１００によれば、第一推定部１４０が推定した角速度の誤差ｂｇと姿勢角の誤差εとに基づいて、角速度計測部（角速度センサ１１１）が計測した角速度ωｂを角速度誤差補正部１２１が補正し、姿勢角外挿計算部１２２が計算した姿勢角ｑｎを姿勢角誤差補正部１２３が補正するので、移動体８００の状態を精度よく推定することができる。
【００８８】
この実施の形態における航法装置１００において、上記第一推定部１４０は、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを状態量とするカルマンフィルタにより、上記状態量を推定する。
【００８９】
この実施の形態における航法装置１００によれば、第一推定部１４０がカルマンフィルタにより状態量を推定するので、状態量の成分が少ないことにより計算量が大幅に削減できる。
【００９０】
この実施の形態における航法装置１００において、上記第一推定部１４０は、以下の式で表される観測量ベクトルΔｚと、状態量ベクトルｘと、推移行列Φと、観測行列Ｈとを用いたカルマンフィルタにより上記状態量を推定する。
【数２８】
ただし、ｄｖは、上記速度減算部１３０が計算した速度の差を表わす三次の縦ベクトル。εは、上記第一推定部１４０が推定する角速度の誤差を表わす三次の縦ベクトル。ｂｇは、上記第一推定部１４０が推定する姿勢角の誤差を表わす三次の縦ベクトル。Ｉ３×３は、三行三列の単位行列。０３×３は、三行三列の零行列。Ｔｓは、上記速度計測部（ＧＰＳ受信機１１３）が速度を計測する周期を表わすスカラー値。Ｃｎｂは、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎに基づいて、上記第一座標系を、上記第二座標系へ変換する座標変換を表わす三行三列の方向余弦行列。Δｖｎ＝［Δｖｎｘ Δｖｎｙ Δｖｎｚ］は、上記速度増分計算部１２６が計算した速度の増分を表わす三次のベクトル。
【００９１】
この実施の形態における航法装置１００によれば、加速度計測部（加速度センサ１１２）が計測した第一座標系（移動体座標系）における加速度ａｂを、姿勢角外挿計算部１２２が計算した姿勢角ｑｎに基づいて、速度増分計算部１２６が座標変換し、座標変換した加速度に基づいて速度の増分Δｖｎを算出し、算出した速度の増分Δｖｎの成分を、観測行列Ｈの成分とするので、推定すべき状態量である姿勢角の誤差εと、観測量である速度の差ｄｖとを結びつけることができ、速度の誤差などを状態量として推定する必要がないので、カルマンフィルタで使用する各行列の次数を小さくすることができ、カルマンフィルタの計算に必要な計算量を少なくすることができる。、
【００９２】
この実施の形態における航法装置１００は、更に、速度誤差推定部１５２と、速度誤差補正部１２８とを有する。
上記速度誤差推定部１５２は、上記速度減算部１３０が計算した速度の差ｄｖに基づいて、上記速度外挿計算部１２７が計算した速度の誤差δｖを推定する。
上記速度誤差補正部１２８は、上記速度誤差推定部１５２が推定した誤差δｖに基づいて、上記速度外挿計算部１２７が計算した速度ｖｎを補正する。
【００９３】
この実施の形態における航法装置１００によれば、速度減算部１３０が計算した速度の差ｄｖに基づいて、速度誤差推定部１５２が速度の誤差δｖを推定するので、カルマンフィルタのような計算量の多い推定方式を用いる必要がなく、推定に必要な計算の計算量を少なくすることができる。
【００９４】
この実施の形態における航法装置１００において、上記速度誤差推定部１５２は、上記速度減算部１３０が計算した速度の差ｄｖに、所定のゲインＫｖを乗じ、上記速度外挿計算部１２７が計算した速度の誤差δｖとする。
【００９５】
この実施の形態における航法装置１００によれば、速度減算部１３０が計算した速度の差ｄｖに、速度誤差推定部１５２が所定のゲインＫｖを乗じることにより、速度の誤差δｖを推定するので、極めて少ない計算量で、速度の誤差δｖを推定することができる。
【００９６】
この実施の形態における航法装置１００は、更に、速度計測精度算出部（ＧＰＳ受信機１１３）と、ゲイン算出部１５１とを有する。
上記速度計測精度算出部（ＧＰＳ受信機１１３）は、上記速度計測部（ＧＰＳ受信機１１３）が計測した速度の精度を算出する。
上記ゲイン算出部１５１は、上記速度計測精度算出部（ＧＰＳ受信機１１３）が算出した精度に基づいて、ゲインＫｖを算出する。
上記速度誤差推定部１５２は、上記速度減算部１３０が計算した速度の差ｄｖに、上記ゲイン算出部１５１が算出したゲインＫｖを乗じ、上記速度外挿計算部１２７が計算した速度の誤差δｖとする。
【００９７】
この実施の形態における航法装置１００によれば、速度計測精度算出部（ＧＰＳ受信機１１３）が算出した精度に基づいて、ゲイン算出部１５１がゲインＫｖを算出するので、速度計測部（ＧＰＳ受信機１１３）の計測精度に応じて、ゲインＫｖを調整することができ、速度の誤差δｖの推定の妥当性を高くすることができる。
【００９８】
この実施の形態における航法装置１００が、推定をする推定方法は、以下の工程を有する。
上記角速度計測部（角速度センサ１１１）が、上記航法装置１００が搭載された移動体８００を基準とした第一座標系（移動体座標系）における上記移動体８００の角速度ωｂを計測する。
上記加速度計測部（加速度センサ１１２）が、上記第一座標系における上記移動体８００に加わる加速度ａｂを計測する。
上記速度計測部（ＧＰＳ受信機１１３）が、上記第一座標系と異なる第二座標系（慣性座標系）における上記移動体８００の速度ｖｇを計測する。
上記姿勢角外挿計算部１２２が、上記角速度計測部（角速度センサ１１１）が計測した角速度ωｂに基づいて、上記第二座標系における上記移動体８００の姿勢角ｑｎを計算する。
上記速度増分計算部１２６が、上記加速度計測部（加速度センサ１１２）が計測した加速度ａｂと、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎとに基づいて、上記第二座標系における上記移動体８００の速度の増分Δｖｎを計算する。
上記速度外挿計算部１２７が、上記速度増分計算部１２６が計算した速度の増分Δｖｎに基づいて、上記第二座標系における上記移動体８００の速度ｖｎを計算する。
上記速度減算部１３０が、上記速度外挿計算部１２７が計算した速度ｖｎと、上記速度計測部（ＧＰＳ受信機１１３）が計測した速度ｖｇとの差ｄｖを計算する。
上記第一推定部１４０が、上記姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、上記速度増分計算部１２６が計算した速度の増分Δｖｎと、上記速度減算部１３０が計算した速度の差ｄｖとに基づいて、上記角速度計測部（角速度センサ１１１）が計測した角速度の誤差ｂｇと、上記姿勢角外挿計算部１２２が計算した姿勢角の誤差εとを推定する。
【００９９】
この実施の形態における推定方法によれば、姿勢角外挿計算部１２２が計算した姿勢角ｑｎと、速度増分計算部１２６が計算した速度の増分Δｖｎと、速度減算部１３０が計算した速度の差ｄｖとに基づいて、第一推定部１４０が角速度の誤差ｂｇと、姿勢角の誤差εとを推定するので、推定にかかる計算量を少なくすることができる。
【０１００】
以上説明した航法装置１００は、航空機等の加速度運動を伴う移動体８００に搭載され、移動体８００の三次元姿勢角ｑ及び速度ｖの推定を行う。航法装置１００は、ＧＰＳ速度出力ｖｇと慣性航法計算に基づく速度ｖｎとの差ｄｖを観測量Δｚとし、姿勢角誤差εおよび角速度バイアス値ｂｇを推定・補正する推定フィルタ（第一推定部１４０）と、同じく観測量（速度の差ｄｖ）に基づき、速度誤差δｖを推定・補正する第２の推定フィルタ（第二推定部１５０）により、推定フィルタを構成する。これにより、従来方式に比べて著しく推定精度を劣化させることなく、計算負荷を低減することができる。
【０１０１】
航法装置１００は、カルマンフィルタにおける行列計算において、行列の次数を少なくすることにより、計算負荷を低くし、廉価な計算機により航法装置１００を構成できる。これにより、例えば、組込製品等で使用される廉価なマイクロコントローラに実装することができる。
【０１０２】
ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）（角速度計測部・加速度計測部）は、角速度センサと加速度センサをそれぞれ３軸搭載する。ＩＭＵは、直交座標系として定義された移動体の機体座標系（第一座標系）における角速度ベクトルωｂ、加速度ベクトルａｂを計測し、慣性航法部１２０に出力する。
慣性航法部１２０は、適当なアライメント処理およびＧＰＳ等により、速度、姿勢角の初期化を行う。その後、慣性航法部１２０は、ストラップダウン航法計算（外挿計算処理）に基づき、速度ｖｎ、姿勢角ｑｎの更新を行い、出力する。なお、姿勢角の表現形式として、コータニオンに基づく表現形式を例として用いたが、オイラー角、方向余弦行列等、他の表現形式を用いる構成であってもよい。また、ストラップダウン航法計算の計算方式は、上述した計算式によるものに限らず、他の計算方式を用いる構成であってもよい。
【０１０３】
慣性航法部１２０が計算する速度ｖｎ、姿勢角ｑｎは、それぞれの初期化誤差や、ＩＭＵが計測した角速度ベクトルωｂ、加速度ベクトルａｂに含まれるバイアス誤差等の要因により、誤差を発生する。ＩＭＵの計測誤差は積分され、速度や姿勢角の誤差は、時間と共に増加する傾向がある。
【０１０４】
ＧＰＳ受信機１１３は、ＧＰＳ測位に基づく速度ｖｇを出力する。減算器（速度減算部１３０）は、ＧＰＳ受信機１１３の速度出力ｖｇから慣性航法部１２０の速度出力ｖｎを差し引いた差ｄｖを計算する。
第一推定部１４０は、速度差ｄｖと速度出力ｖｎ、姿勢角出力ｑｎから６次元のカルマンフィルタ計算に基づき姿勢角ｑｎの補正εおよび角速度バイアス補正量ｂｇを推定する。
第二推定部１５０は、速度差ｄｖと航法計算部（慣性航法部１２０）の速度出力ｖｎから速度出力ｖｎの誤差を算出し、速度誤差補正部１２８は、速度出力ｖｎの補正を行う。
【０１０５】
第一推定部１４０におけるカルマンフィルタ計算は、状態量として、位置ｐｎ、速度ｖｎ、加速度バイアス補正量ｂａを含んでいない。また、観測量として、位置の差分ｄｐを含んでいない。
航法装置１００は、従来よりも状態量の数を少なくしているが、推定精度の劣化は小さく、実用的に問題がない精度を確保することができる。なぜなら、姿勢角誤差εを推定する基本的な原理は不変であり、廉価な加速度センサにおいても比較的バイアス安定度は高く、姿勢角決定精度は角速度センサの性能に基づくため、加速度センサのバイアス補正量ｂａを省略し、温度補正のみを行った場合でも、性能の劣化を小さく抑えることができるからである。
【０１０６】
第二推定部１５０は、速度の誤差δｖを推定し、速度誤差補正部１２８が速度ｖｎを補正する処理を行う。この計算において、第二推定部１５０は、ゲインＫｖを、例えば、固定、または、ＧＰＳ測位条件に基づいてテーブルにより設定する。これにより、第二推定部１５０における計算負荷は、第一推定部１４０における計算負荷と比べて無視できるレベルとすることができる。
【０１０７】
カルマンフィルタにおける計算負荷は、状態量の次元ｎと観測量の次元ｒに依存する。例えば、状態量の次元ｎを５分の２（１５→６）、観測量の次元ｒを２分の１（６→３）とすれば、１ステップあたりの乗算の数は、１０％以下と著しく減少する。このため、実装において、性能が比較的低いマイクロコンピュータを使用することができ、廉価なシステムを構成することができる。
【０１０８】
角速度計測部（角速度センサ１１１）が計測した角速度ωｂの誤差は、外挿計算により蓄積し、姿勢角外挿計算部１２２が算出する姿勢角ｑｎの誤差に反映する。姿勢角外挿計算部１２２が算出する姿勢角ｑｎの誤差は、外挿計算により蓄積し、速度外挿計算部１２７が算出する速度ｖｎの誤差に反映する。
したがって、角速度ωｂの誤差が姿勢角ｑｎの誤差に反映するのは比較的速いのに対し、姿勢角ｑｎを介する分、角速度ωｂの誤差が速度ｖｎの誤差に反映するのは比較的遅い。このように、誤差の反映する速さが異なる観測量に基づいて、一つのカルマンフィルタで推測しようとすることは、効率が悪い。
以上説明した航法装置１００は、誤差が反映するのが比較的速い部分に絞って、カルマンフィルタによる推測を行うことにより、効率を高くし、必要な計算量を少なくする。
【０１０９】
実施の形態２．
実施の形態２について、図９〜図１４を用いて説明する。
なお、実施の形態１で説明した部分と共通する部分については、同一の符号を付し、説明を省略する場合がある。
【０１１０】
図９は、この実施の形態における航法装置１００の機能ブロックの構成の一例を示すブロック構成図である。
航法装置１００は、実施の形態１で説明した機能ブロックに加えて、更に、位置減算部１６０を有する。
【０１１１】
慣性航法部１２０は、更に、推定した速度ｖｎに基づいて、慣性座標系における移動体８００の位置ｐｎを推定する。
位置減算部１６０は、ＧＰＳ受信機１１３が計測した位置ｐｇと、慣性航法部１２０が推定した位置ｐｎとの差ｄｐ＝ｐｇ−ｐｎを計算する。ここで、位置ｐｇ及び位置ｐｎは、ともに三次の縦ベクトルであるから、位置減算部１６０が計算する位置の差ｄｐも三次の縦ベクトルである。位置減算部１６０は、計算した速度の差ｄｐを表わすデータを出力する。
【０１１２】
第二推定部１５０は、更に、位置減算部１６０が計算した位置の差ｄｐと、ＧＰＳ受信機１１３が推定した精度とに基づいて、慣性航法部１２０が推定した位置ｐｎの誤差δｐを推定する。なお、第二推定部１５０が推定する位置ｐｎの誤差δｐは、例えば、慣性座標系のｘｙｚ三軸それぞれについての誤差を成分とする三次の縦ベクトルである。第二推定部１５０は、更に、推定した位置の誤差δｐを表わすデータを出力する。
【０１１３】
慣性航法部１２０は、更に、第二推定部１５０が推定した位置の誤差δｐに基づいて、推定した位置ｐｎを補正し、誤差を補正した位置ｐを算出する。慣性航法部１２０は、更に、算出した慣性座標系における位置ｐを表わすデータを出力する。
【０１１４】
位置出力部１９３は、慣性航法部１２０が出力した位置ｐを表わすデータを入力する。位置出力部１９３は、入力したデータが表わす位置ｐを、外部に出力する。
【０１１５】
図１０は、この実施の形態における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図である。
慣性航法部１２０は、実施の形態１で説明した機能ブロックに加えて、更に、位置外挿計算部１７１、位置誤差補正部１７２、位置記憶部１７３を有する。
【０１１６】
位置記憶部１７３は、慣性座標系における移動体８００の位置ｐを表わすデータを記憶している。位置記憶部１７３は、例えば、位置ｐを表わすデータとして、慣性座標系におけるｘｙｚ三軸それぞれの座標を表わす三つの実数ｐｘ，ｐｙ，ｐｚを表わすデータを記憶する。
位置記憶部１７３は、あらかじめ、例えば、位置が既知のマーカー上で移動体８００が静止しているときに、マーカーの位置を用いて、記憶した位置ｐを表わすデータを初期化しておく。
【０１１７】
位置外挿計算部１７１は、位置記憶部１７３が記憶した慣性座標系における位置ｐを表わすデータと、速度誤差補正部１２８が出力した補正した速度ｖｎ＋を表わすデータとを入力する。位置外挿計算部１７１は、入力したデータが表わす位置ｐと速度ｖｎ＋とに基づいて、外挿計算をし、慣性座標系における新しい位置ｐｎを算出する。位置外挿計算部１７１は、算出した位置ｐｎを表わすデータを出力する。位置外挿計算部１７１が出力した位置ｐｎを表わすデータは、位置減算部１６０が入力し、位置の差ｄｐの算出に使用する。
位置外挿計算部１７１は、例えば、次の計算式を用いて、位置ｐｎを算出する。
【数２９】
ここで、ｐｎｋは、位置外挿計算部１７１が算出する時刻ｔｋにおける位置を表わす三次の縦ベクトル。ｐｋ−１は、位置記憶部１７３が記憶した時刻ｔｋ−１における位置を表わす三次の縦ベクトル。Ｔｓは、角速度センサ１１１及び加速度センサ１１２が角速度や加速度を計測する周期を表わすスカラー値で、数１２や数１５や数２０におけるＴｓと同じく、Ｔｓ＝ｔｋ−ｔｋ−１。ｖｎ＋ｋは、速度誤差補正部１２８が補正した時刻ｋにおける速度を表わす三次の縦ベクトル。
【０１１８】
位置誤差補正部１７２は、位置外挿計算部１７１が出力した慣性座標系における位置ｐｎを表わすデータと、第二推定部１５０が出力した位置の誤差δｐを表わすデータとを入力する。位置誤差補正部１７２は、入力したデータが表わす位置ｐｎと位置の誤差δｐとに基づいて、位置ｐｎの誤差を補正して、補正した位置ｐｎ＋を算出する。位置誤差補正部１７２は、算出した位置ｐｎ＋を表わすデータを出力する。
位置誤差補正部１７２は、例えば、次の計算式を用いて、補正した位置ｐｎ＋を算出する。
【数３０】
ここで、ｐｎ＋ｋは、位置誤差補正部１７２が算出する時刻ｔｋにおける補正した位置を表わす三次の縦ベクトル。ｐｎｋは、位置外挿計算部１７１が算出した時刻ｔｋにおける位置を表わす三次の縦ベクトル。δｐは、第二推定部１５０が推定した位置の誤差を表わす三次の縦ベクトル。
【０１１９】
なお、ＧＰＳ受信機１１３が移動体８００の位置・速度を計測していない場合、位置誤差補正部１７２は、位置誤差の補正をせず、入力したデータが表わす位置ｐｎを、そのまま位置ｐｎ＋とする。
【０１２０】
位置記憶部１７３は、位置誤差補正部１７２が出力した補正した位置ｐｎ＋を表わすデータを入力する。位置記憶部１７３は、入力したデータを、位置ｐを表わすデータとして記憶する。位置記憶部１７３が記憶した位置ｐを表わすデータは、位置出力部１９３が入力して、外部に出力する。
【０１２１】
図１１は、この実施の形態における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図である。
第二推定部１５０は、実施の形態１で説明した機能ブロックに加えて、更に、位置誤差推定部１５３を有する。
【０１２２】
ゲイン算出部１５１は、更に、ＧＰＳ受信機１１３が推定した位置の計測の精度に基づいて、位置誤差のゲインＫｐを算出する。ゲイン算出部１５１は、算出したゲインＫｐを表わすデータを出力する。
ゲイン算出部１５１が算出するゲインＫｐは、ゲインＫｖと同様の値であり、ゲインＫｖと同じであってもよいし、異なっていてもよい。
【０１２３】
位置誤差推定部１５３は、位置減算部１６０が出力した位置の差ｄｐを表わすデータと、ゲイン算出部１５１が出力したゲインＫｐを表わすデータとを入力する。位置誤差推定部１５３は、入力したデータが表わす位置の差ｄｐとゲインＫｐとに基づいて、慣性航法部１２０が推定した位置の誤差δｐを算出する。位置誤差推定部１５３は、算出した位置の誤差δｐを表わすデータを出力する。位置誤差推定部１５３が出力した位置の誤差δｐを表わすデータは、慣性航法部１２０が入力し、誤差の補正に使用する。
位置誤差推定部１５３は、例えば、次の計算式を用いて、位置の誤差δｐを算出する。
【数３１】
ここで、δｐは、位置誤差推定部１５３が算出する位置の誤差を表わす三次の縦ベクトル。Ｋｐは、ゲイン算出部１５１が算出した位置誤差のゲインを表わすスカラー値もしくは三行三列の行列。ｄｐは、位置減算部１６０が算出した位置の差を表わす三次の縦ベクトル。
【０１２４】
次に、動作について説明する。
この実施の形態における航法処理の全体的な流れは、実施の形態１で説明したものと同様である。
【０１２５】
図１２は、この実施の形態における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図である。
初期化処理Ｓ５１０は、実施の形態１で説明した工程に加えて、更に、位置初期化工程Ｓ５１３を有する。
【０１２６】
位置初期化工程Ｓ５１３において、位置記憶部１７３は、記憶した位置ｐを初期化する。
【０１２７】
図１３は、この実施の形態における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図である。
外挿計算処理Ｓ５２０は、実施の形態１で説明した工程に加えて、更に、位置外挿計算工程Ｓ５２７を有する。
【０１２８】
位置外挿計算工程Ｓ５２７において、位置外挿計算部１７１は、速度外挿計算工程Ｓ５２６で速度外挿計算部１２７が算出した速度ｖｎに基づいて、位置記憶部１７３が記憶した位置ｐを外挿し、外挿した位置ｐｎを算出する。ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、位置外挿計算部１７１が算出した外挿した位置ｐｎが、そのまま位置記憶部１７３が記憶する位置ｐとなるので、位置記憶部１７３は、位置外挿計算部１７１が外挿した位置ｐｎを、暫定的に、位置ｐとして記憶する。
【０１２９】
図１４は、この実施の形態における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図である。
観測更新処理Ｓ５５０は、実施の形態１で説明した工程に加えて、更に、位置減算工程Ｓ５６４、位置誤差推定工程Ｓ５６５、位置誤差補正工程Ｓ５６６を有する。
【０１３０】
ゲイン算出工程Ｓ５６１において、ゲイン算出部１５１は、ＧＰＳ受信機１１３が推定した計測の精度に基づいて、速度誤差のゲインＫｖと、位置誤差のゲインＫｐとを算出する。
【０１３１】
位置減算工程Ｓ５６４において、位置減算部１６０は、ＧＰＳ受信機１１３が計測した位置ｐｇと、位置外挿計算工程Ｓ５２７で位置外挿計算部１７１が外挿した位置ｐｎとに基づいて、位置の差ｄｐを算出する。
位置誤差推定工程Ｓ５６５において、位置誤差推定部１５３は、ゲイン算出工程Ｓ５６１でゲイン算出部１５１が算出した位置誤差のゲインＫｐと、位置減算工程Ｓ５６４で位置減算部１６０が算出した位置の差ｄｐとに基づいて、位置の誤差δｐを算出する。
位置誤差補正工程Ｓ５６６において、位置誤差補正部１７２は、位置誤差推定工程Ｓ５６５で位置誤差推定部１５３が算出した位置の誤差δｐに基づいて、位置外挿計算工程Ｓ５２７で位置外挿計算部１７１が外挿した位置ｐｎを補正し、補正した位置ｐｎ＋を算出する。位置記憶部１７３は、位置誤差補正部１７２が補正した位置ｐｎ＋を、位置ｐとして記憶する。
なお、ＧＰＳ受信機１１３が移動体８００の位置や速度を計測していない場合、観測更新処理Ｓ５５０を実行しないので、位置記憶部１７３は、位置外挿計算工程Ｓ５２７で暫定的に記憶した位置ｐ（すなわち、位置外挿計算部１７１が外挿した位置ｐｎ）を、そのまま記憶し続けることになる。
【０１３２】
この実施の形態における航法装置１００は、更に、位置計測部（ＧＰＳ受信機１１３）と、位置外挿計算部１７１と、位置減算部１６０と、位置誤差推定部１５３と、位置誤差補正部１７２とを有する。
上記位置計測部（ＧＰＳ受信機１１３）は、上記第二座標系（慣性座標系）における移動体８００の位置ｐｇを計測する。
上記位置外挿計算部１７１は、上記速度外挿計算部１２７が計算した速度ｖｎに基づいて、上記第二座標系における移動体８００の位置ｐｎを計算する。
上記位置減算部１６０は、上記位置外挿計算部１７１が計算した位置ｐｎと、上記位置計測部（ＧＰＳ受信機１１３）が計測した位置ｐｇとの差を計算する。
上記位置誤差推定部１５３は、上記位置減算部１６０が計算した位置の差ｄｐに基づいて、上記位置外挿計算部１７１が計算した位置の誤差δｐを推定する。
上記位置誤差補正部１７２は、上記位置誤差推定部１５３が推定した位置の誤差δｐに基づいて、上記位置外挿計算部１７１が計算した位置ｐｎを補正する。
【０１３３】
この実施の形態における航法装置１００によれば、速度外挿計算部１２７が算出した移動体８００の速度ｖｎに基づいて、位置外挿計算部１７１が移動体８００の位置ｐｎを算出するので、なんらかの原因で、位置計測部（ＧＰＳ受信機１１３）が移動体８００の位置ｐｇを計測できない場合であっても、移動体８００の位置を推定することができる。
【０１３４】
以上説明した航法装置１００は、慣性航法部１２０において位置外挿計算を実施し、第二推定部１５０において位置に関する更新計算も行う。航法装置１００は、慣性航法部１２０が、位置外挿計算を実施し、減算器（速度減算部１３０・位置減算部１６０）が、位置・速度の差ｄｐ、ｄｖを計算し、第二推定部１５０が、位置に関する更新計算も行う。なお、位置誤差のゲインＫｐは、速度誤差のゲインＫｖと同様に固定または簡単な計算により設定することができる。
これにより、ＧＰＳ出力が一時中断した場合やＧＰＳ出力の遅延が大きいことが問題となる場合など、慣性装置に基づく位置出力ｐｎを使用することができる。
【図面の簡単な説明】
【０１３５】
【図１】実施の形態１における航法装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図２】実施の形態１における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図。
【図３】実施の形態１における第一推定部１４０の機能ブロックの構成の一例を示す詳細ブロック図。
【図４】実施の形態１における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図。
【図５】実施の形態１における航法装置１００が移動体８００の速度・姿勢角を推定する航法処理の全体的な流れの一例を示すフローチャート図。
【図６】実施の形態１における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図。
【図７】実施の形態１における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図。
【図８】実施の形態１における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図。
【図９】実施の形態２における航法装置１００の機能ブロックの構成の一例を示すブロック構成図。
【図１０】実施の形態２における慣性航法部１２０の機能ブロックの構成の一例を示す詳細ブロック図。
【図１１】実施の形態２における第二推定部１５０の機能ブロックの構成の一例を示す詳細ブロック図。
【図１２】実施の形態２における航法装置１００がデータを初期化する初期化処理Ｓ５１０の詳細な流れの一例を示すフローチャート図。
【図１３】実施の形態２における航法装置１００が外挿計算をする外挿計算処理Ｓ５２０の詳細な流れの一例を示すフローチャート図。
【図１４】実施の形態２における航法装置１００が観測更新をする観測更新処理Ｓ５５０の詳細な流れの一例を示すフローチャート図。
【符号の説明】
【０１３６】
１００ 航法装置、１１１ 角速度センサ、１１２ 加速度センサ、１１３ ＧＰＳ受信機、１２０ 慣性航法部、１２１ 角速度誤差補正部、１２２ 姿勢角外挿計算部、１２３ 姿勢角誤差補正部、１２４ 姿勢角記憶部、１２５ 方向余弦行列計算部、１２６ 速度増分計算部、１２７ 速度外挿計算部、１２８ 速度誤差補正部、１２９ 速度記憶部、１３０ 速度減算部、１４０ 第一推定部、１４１ 観測行列計算部、１４２ 推移行列計算部、１４３ 誤差共分散外挿計算部、１４４ カルマンゲイン計算部、１４５ 誤差共分散観測更新計算部、１４６ 誤差共分散記憶部、１４７ 状態量外挿計算部、１４８ 状態量観測更新計算部、１４９ 状態量記憶部、１５０ 第二推定部、１５１ ゲイン算出部、１５２ 速度誤差推定部、１５３ 位置誤差推定部、１６０ 位置減算部、１７１ 位置外挿計算部、１７２ 位置誤差補正部、１７３ 位置記憶部、１９１ 姿勢角出力部、１９２ 速度出力部、１９３ 位置出力部、８００ 移動体。
【特許請求の範囲】
【請求項１】
移動体に搭載される航法装置において、
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有し、
上記角速度計測部は、上記移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部は、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部は、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部は、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部は、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部は、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部は、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする航法装置。
【請求項２】
上記航法装置は、更に、角速度誤差補正部と、姿勢角誤差補正部とを有し、
上記角速度誤差補正部は、上記第一推定部が推定した角速度の誤差に基づいて、上記角速度計測部が計測した角速度を補正し、
上記姿勢角外挿計算部は、上記角速度誤差補正部が補正した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記姿勢角誤差補正部は、上記第一推定部が推定した姿勢角の誤差に基づいて、上記姿勢角外挿計算部が計算した姿勢角を補正し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角誤差補正部が補正した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記第一推定部は、上記姿勢角誤差補正部が補正した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする請求項１に記載の航法装置。
【請求項３】
上記第一推定部は、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを状態量とするカルマンフィルタにより、上記状態量を推定することを特徴とする請求項１または請求項２に記載の航法装置。
【請求項４】
上記第一推定部は、以下の式で表される観測量ベクトルΔｚと、状態量ベクトルｘと、推移行列Φと、観測行列Ｈとを用いたカルマンフィルタにより上記状態量を推定することを特徴とする請求項３に記載の航法装置。
【数１】
ただし、ｄｖは、上記速度減算部が計算した速度の差を表わす三次の縦ベクトル。εは、上記第一推定部が推定する角速度の誤差を表わす三次の縦ベクトル。ｂｇは、上記第一推定部が推定する姿勢角の誤差を表わす三次の縦ベクトル。Ｉ３×３は、三行三列の単位行列。０３×３は、三行三列の零行列。Ｔｓは、上記速度計測部が速度を計測する周期を表わすスカラー値。Ｃｎｂは、上記姿勢角外挿計算部が計算した姿勢角に基づいて、上記第一座標系を、上記第二座標系へ変換する座標変換を表わす三行三列の方向余弦行列。Δｖｎ＝［Δｖｎｘ Δｖｎｙ Δｖｎｚ］は、上記速度増分計算部が計算した速度の増分を表わす三次のベクトル。
【請求項５】
上記航法装置は、更に、速度誤差推定部と、速度誤差補正部とを有し、
上記速度誤差推定部は、上記速度減算部が計算した速度の差に基づいて、上記速度外挿計算部が計算した速度の誤差を推定し、
上記速度誤差補正部は、上記速度誤差推定部が推定した誤差に基づいて、上記速度外挿計算部が計算した速度を補正することを特徴とする請求項１乃至請求項４のいずれかに記載の航法装置。
【請求項６】
上記速度誤差推定部は、上記速度減算部が計算した速度の差に、所定のゲインを乗じ、上記速度外挿計算部が計算した速度の誤差とすることを特徴とする請求項５に記載の航法装置。
【請求項７】
上記航法装置は、更に、速度計測精度算出部と、ゲイン算出部とを有し、
上記速度計測精度算出部は、上記速度計測部が計測した速度の精度を算出し、
上記ゲイン算出部は、上記速度計測精度算出部が算出した精度に基づいて、ゲインを算出し、
上記速度誤差推定部は、上記速度減算部が計算した速度の差に、上記ゲイン算出部が算出したゲインを乗じ、上記速度外挿計算部が計算した速度の誤差とすることを特徴とする請求項５または請求項６に記載の航法装置。
【請求項８】
上記航法装置は、更に、位置計測部と、位置外挿計算部と、位置減算部と、位置誤差推定部と、位置誤差補正部とを有し、
上記位置計測部は、上記第二座標系における移動体の位置を計測し、
上記位置外挿計算部は、上記速度外挿計算部が計算した速度に基づいて、上記第二座標系における移動体の位置を計算し、
上記位置減算部は、上記位置外挿計算部が計算した位置と、上記位置計測部が計測した位置との差を計算し、
上記位置誤差推定部は、上記位置減算部が計算した位置の差に基づいて、上記位置外挿計算部が計算した位置の誤差を推定し、
上記位置誤差補正部は、上記位置誤差推定部が推定した位置の誤差に基づいて、上記位置外挿計算部が計算した位置を補正することを特徴とする請求項１乃至請求項７のいずれかに記載の航法装置。
【請求項９】
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有する航法装置が、推定をする推定方法において、
上記角速度計測部が、上記航法装置が搭載された移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部が、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部が、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部が、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部が、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部が、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部が、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部が、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする推定方法。
【請求項１】
移動体に搭載される航法装置において、
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有し、
上記角速度計測部は、上記移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部は、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部は、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部は、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部は、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部は、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部は、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする航法装置。
【請求項２】
上記航法装置は、更に、角速度誤差補正部と、姿勢角誤差補正部とを有し、
上記角速度誤差補正部は、上記第一推定部が推定した角速度の誤差に基づいて、上記角速度計測部が計測した角速度を補正し、
上記姿勢角外挿計算部は、上記角速度誤差補正部が補正した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記姿勢角誤差補正部は、上記第一推定部が推定した姿勢角の誤差に基づいて、上記姿勢角外挿計算部が計算した姿勢角を補正し、
上記速度増分計算部は、上記加速度計測部が計測した加速度と、上記姿勢角誤差補正部が補正した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記第一推定部は、上記姿勢角誤差補正部が補正した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする請求項１に記載の航法装置。
【請求項３】
上記第一推定部は、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを状態量とするカルマンフィルタにより、上記状態量を推定することを特徴とする請求項１または請求項２に記載の航法装置。
【請求項４】
上記第一推定部は、以下の式で表される観測量ベクトルΔｚと、状態量ベクトルｘと、推移行列Φと、観測行列Ｈとを用いたカルマンフィルタにより上記状態量を推定することを特徴とする請求項３に記載の航法装置。
【数１】
ただし、ｄｖは、上記速度減算部が計算した速度の差を表わす三次の縦ベクトル。εは、上記第一推定部が推定する角速度の誤差を表わす三次の縦ベクトル。ｂｇは、上記第一推定部が推定する姿勢角の誤差を表わす三次の縦ベクトル。Ｉ３×３は、三行三列の単位行列。０３×３は、三行三列の零行列。Ｔｓは、上記速度計測部が速度を計測する周期を表わすスカラー値。Ｃｎｂは、上記姿勢角外挿計算部が計算した姿勢角に基づいて、上記第一座標系を、上記第二座標系へ変換する座標変換を表わす三行三列の方向余弦行列。Δｖｎ＝［Δｖｎｘ Δｖｎｙ Δｖｎｚ］は、上記速度増分計算部が計算した速度の増分を表わす三次のベクトル。
【請求項５】
上記航法装置は、更に、速度誤差推定部と、速度誤差補正部とを有し、
上記速度誤差推定部は、上記速度減算部が計算した速度の差に基づいて、上記速度外挿計算部が計算した速度の誤差を推定し、
上記速度誤差補正部は、上記速度誤差推定部が推定した誤差に基づいて、上記速度外挿計算部が計算した速度を補正することを特徴とする請求項１乃至請求項４のいずれかに記載の航法装置。
【請求項６】
上記速度誤差推定部は、上記速度減算部が計算した速度の差に、所定のゲインを乗じ、上記速度外挿計算部が計算した速度の誤差とすることを特徴とする請求項５に記載の航法装置。
【請求項７】
上記航法装置は、更に、速度計測精度算出部と、ゲイン算出部とを有し、
上記速度計測精度算出部は、上記速度計測部が計測した速度の精度を算出し、
上記ゲイン算出部は、上記速度計測精度算出部が算出した精度に基づいて、ゲインを算出し、
上記速度誤差推定部は、上記速度減算部が計算した速度の差に、上記ゲイン算出部が算出したゲインを乗じ、上記速度外挿計算部が計算した速度の誤差とすることを特徴とする請求項５または請求項６に記載の航法装置。
【請求項８】
上記航法装置は、更に、位置計測部と、位置外挿計算部と、位置減算部と、位置誤差推定部と、位置誤差補正部とを有し、
上記位置計測部は、上記第二座標系における移動体の位置を計測し、
上記位置外挿計算部は、上記速度外挿計算部が計算した速度に基づいて、上記第二座標系における移動体の位置を計算し、
上記位置減算部は、上記位置外挿計算部が計算した位置と、上記位置計測部が計測した位置との差を計算し、
上記位置誤差推定部は、上記位置減算部が計算した位置の差に基づいて、上記位置外挿計算部が計算した位置の誤差を推定し、
上記位置誤差補正部は、上記位置誤差推定部が推定した位置の誤差に基づいて、上記位置外挿計算部が計算した位置を補正することを特徴とする請求項１乃至請求項７のいずれかに記載の航法装置。
【請求項９】
角速度計測部と、加速度計測部と、速度計測部と、姿勢角外挿計算部と、速度増分計算部と、速度外挿計算部と、速度減算部と、第一推定部とを有する航法装置が、推定をする推定方法において、
上記角速度計測部が、上記航法装置が搭載された移動体を基準とした第一座標系における上記移動体の角速度を計測し、
上記加速度計測部が、上記第一座標系における上記移動体に加わる加速度を計測し、
上記速度計測部が、上記第一座標系と異なる第二座標系における上記移動体の速度を計測し、
上記姿勢角外挿計算部が、上記角速度計測部が計測した角速度に基づいて、上記第二座標系における上記移動体の姿勢角を計算し、
上記速度増分計算部が、上記加速度計測部が計測した加速度と、上記姿勢角外挿計算部が計算した姿勢角とに基づいて、上記第二座標系における上記移動体の速度の増分を計算し、
上記速度外挿計算部が、上記速度増分計算部が計算した速度の増分に基づいて、上記第二座標系における上記移動体の速度を計算し、
上記速度減算部が、上記速度外挿計算部が計算した速度と、上記速度計測部が計測した速度との差を計算し、
上記第一推定部が、上記姿勢角外挿計算部が計算した姿勢角と、上記速度増分計算部が計算した速度の増分と、上記速度減算部が計算した速度の差とに基づいて、上記角速度計測部が計測した角速度の誤差と、上記姿勢角外挿計算部が計算した姿勢角の誤差とを推定することを特徴とする推定方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１０−９６６４７（Ｐ２０１０−９６６４７Ａ）
【公開日】平成２２年４月３０日（２０１０．４．３０）
【国際特許分類】
【出願番号】特願２００８−２６８１５０（Ｐ２００８−２６８１５０）
【出願日】平成２０年１０月１７日（２００８．１０．１７）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２２年４月３０日（２０１０．４．３０）
【国際特許分類】
【出願日】平成２０年１０月１７日（２００８．１０．１７）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]