状態推定装置、電子機器及びプログラム
【課題】 カルマンフィルターの内部状態変数と観測量の補正処理により、推定精度を維持することができる状態推定装置の提供。
【解決手段】 状態推定装置１００は、観測信号に基づきシステムの内部状態をカルマンフィルターにより推定する状態推定部１８０と、観測信号を観測量に変換する信号変換部１２０と、観測量の補正処理を行う補正部１６０と、補正部１６０と状態推定部１８０を制御する制御部１４０とを含む。制御部１４０は、所定の補正条件が成立したと判断する場合、又は所定の補正条件が成立した旨を通知された場合に、補正部１６０及び状態推定部１８０に補正指示を行う。補正部１６０は、補正指示が行われた場合に、観測量の補正処理を行う。状態推定部１８０は、補正指示が行われた場合に、観測量に対応する内部状態変数の補正処理を行い、補正後の観測量と内部状態変数とに基づき状態推定を行う。
【解決手段】 状態推定装置１００は、観測信号に基づきシステムの内部状態をカルマンフィルターにより推定する状態推定部１８０と、観測信号を観測量に変換する信号変換部１２０と、観測量の補正処理を行う補正部１６０と、補正部１６０と状態推定部１８０を制御する制御部１４０とを含む。制御部１４０は、所定の補正条件が成立したと判断する場合、又は所定の補正条件が成立した旨を通知された場合に、補正部１６０及び状態推定部１８０に補正指示を行う。補正部１６０は、補正指示が行われた場合に、観測量の補正処理を行う。状態推定部１８０は、補正指示が行われた場合に、観測量に対応する内部状態変数の補正処理を行い、補正後の観測量と内部状態変数とに基づき状態推定を行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置、電子機器及びプログラム等に関係する。
【背景技術】
【０００２】
近年、方角や位置等を求めるシステムとして、測位システムや慣性航法システムが広く知られている。
【０００３】
測位システムとしては、ＧＰＳ（Global Positioning System）が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。ＧＰＳでは、複数のＧＰＳ衛星の位置や、各ＧＰＳ衛星から位置算出装置までの擬似距離等の情報に基づいて、位置算出装置の位置等を算出する。また、位置算出装置の移動前の位置と移動後の位置とのベクトルを求めることにより、位置算出装置の移動している方角を推定することもできる。
【０００４】
一方、慣性航法システムは、外部からの電波による支援を得ることなく、自らに搭載する地磁気センサーや、ジャイロセンサー、加速度センサー等からの情報のみによって、自らの位置や速度、姿勢、向き等を推定する。地球上での絶対位置が分かるＧＰＳに対して、慣性航法システムは初期位置からの相対位置を推定することができる。
【０００５】
最近では、ＧＰＳや慣性航法システムを利用して、ユーザーの道案内をするサービス等も行われている。
【０００６】
これらのＧＰＳや慣性航法システムに関する発明としては特許文献１、２に記載される従来技術がある。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００６−１２６１４８号公報
【特許文献２】特開２００７−２７６５０７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ＧＰＳは、電波状況に依存するため、地下や建物の中などでは位置や方角を正確に推定することができない。そのため、先に述べたＧＰＳを利用する道案内サービスでは、ユーザーが地下や建物の中に入ってしまうと、十分な精度を保ちつつ、サービスを提供することができないという点が問題となる。
【０００９】
一方、慣性航法システムを利用した道案内サービスは、電波状況には依存しないため、地下街等においても、精度を保ったまま道案内サービスを提供することができ、上記の問題を解決することができる。
【００１０】
しかし、慣性航法システムは、地磁気センサーを用いて、方角を推定するため、地磁気環境の悪い場所では誤った方角を推定してしまうという問題がある。例えば、地磁気センサーを携帯電話やＰＣなどに近づけると、外部の磁場を検出してしまい、大きく誤った方角を検出してしまう。
【００１１】
本発明の幾つかの態様によれば、観測量と、カルマンフィルターの内部状態変数との補正処理を行うことにより、誤った推定をした後も、元の推定精度を維持することができる状態推定装置、電子機器及びプログラム等を提供できる。
【課題を解決するための手段】
【００１２】
本発明の一態様は、観測により入力される観測信号に基づいて、前記観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部と、前記観測信号を観測量に変換し、前記観測量を補正部に出力する信号変換部と、前記観測量の補正処理を行い、補正処理後の前記観測量を前記状態推定部に出力する補正部と、前記補正部と前記状態推定部を制御する制御部と、を含み、前記制御部は、所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、前記補正部は、前記補正指示が行われた場合に、前記観測量の補正処理を行い、前記状態推定部は、前記補正指示が行われた場合に、前記観測量に対応する内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とする状態推定装置に関係する。また本発明の他の態様は、上記各部としてコンピューターを機能させるプログラム、又は該プログラムを記憶したコンピューター読み取り可能な情報記憶媒体に関係する。
【００１３】
本発明の一態様では、入力された観測信号を観測量へ変換する。次に、本発明の一態様では、補正条件が成立したと判断する場合、又は補正条件が成立した旨を通知された場合に、観測量と内部状態変数の補正指示が行われ、これらの値を補正する。これにより、補正した観測量を入力とし、内部状態変数を補正した状態で、カルマンフィルターにより、状態推定を行うことができる。
【００１４】
また、本発明の一態様では、前記信号変換部は、方位センサーからのセンサー信号を含む観測信号を、磁北を基準とする方位角の情報を含む観測量に変換し、前記制御部は、前記所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、方位角補正データを通知し、前記補正部は、前記補正指示が行われた場合に、前記制御部から通知された前記方位角補正データと、磁北を真北に補正する偏角とに基づいて、前記観測量の補正処理を行い、補正後の前記観測量を前記状態推定部に出力し、前記状態推定部は、前記補正指示が行われた場合に、前記方位角補正データに基づいて、前記観測量に対応する前記内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行ってもよい。
【００１５】
これにより、方位センサーからのセンサー信号を含む観測信号を、方位角の情報を含む観測量に変換することができる。そして、方位角の情報を含む観測量と内部状態変数を補正しながら、状態推定を行うことが可能になる。
【００１６】
また、本発明の一態様では、前記補正部は、前記補正指示が行われた場合には、前記方位角補正データと前記偏角とに基づいて、前記観測量の補正処理を行い、前記補正指示が行われない場合には、前記偏角に基づいて、前記観測量の補正処理を行ってもよい。
【００１７】
これにより、補正指示が行われた場合には、方位角補正データと偏角とに基づいて観測量を補正し、補正指示が行われない場合には、偏角によって観測量を補正して、状態推定を行うことができる。
【００１８】
また、本発明の一態様では、前記状態推定部は、前記補正指示が行われた場合には、前記方位角補正データに基づいて、前記内部状態変数の補正処理を行い、前記補正指示が行われない場合には、前記内部状態変数の補正処理を行わなくてもよい。
【００１９】
これにより、補正指示が行われない場合に、前回更新した値に補正を加えていない内部状態変数と、偏角により補正された観測量とを用いて、状態推定を行うこと等が可能となる。
【００２０】
また、本発明の一態様では、前記信号変換部は、方位センサーから通知された観測信号に基づいて、全磁力を算出し、前記全磁力を前記制御部に通知し、前記制御部は、通知された前記全磁力に基づいて、前記所定の補正条件の判断をし、前記全磁力が所定の閾値により特定される範囲内にない場合には、所定の補正条件を満たすと判断し、前記全磁力が前記所定の閾値の範囲内にある場合には、所定の補正条件を満たさないと判断してもよい。
【００２１】
これにより、方位センサーにより検出された観測信号に基づき、全磁力を算出することができ、その全磁力に基づいて、補正処理を行うか否かを決定すること等ができる。
【００２２】
また、本発明の一態様では、前記信号変換部は、前記方位センサーから通知された観測信号に基づいて、伏角を算出し、前記伏角を前記制御部に通知し、前記制御部は、通知された前記伏角に基づいて、前記所定の補正条件の判断をし、前記伏角が所定の閾値により特定される範囲内にない場合には、前記所定の補正条件を満たすと判断し、前記伏角が前記所定の閾値の範囲内にある場合には、前記所定の補正条件を満たさないと判断してもよい。
【００２３】
これにより、方位センサーにより検出された観測信号に基づき、伏角を算出することができ、その伏角に基づいて、補正処理を行うか否かを決定すること等ができる。
【００２４】
また、本発明の一態様では、前記制御部は、前記信号変換部から取得した前記観測量をマップマッチング部に出力し、マップマッチング部からマップマッチングの結果を取得し、前記マップマッチングの結果に基づいて、前記所定の補正条件を判断してもよい。
【００２５】
これにより、マップマッチングの結果に基づいて、補正処理を行うか否かを決定すること等ができる。
【００２６】
また、本発明の一態様では、前記制御部は、操作部から入力されたユーザー入力情報に基づいて、前記方位角補正データを特定し、前記補正部及び前記状態推定部に前記補正指示と前記方位角補正データとを通知し、前記補正部は、前記補正指示が行われた場合には、前記方位角補正データと、前記偏角とに基づいて、前記観測量の補正処理を行い、前記状態推定部は、前記補正指示が行われた場合には、前記方位角補正データに基づいて、前記内部状態変数の補正処理を行ってもよい。
【００２７】
これにより、ユーザー入力情報に基づいて、方位角補正データを特定することができる。そして、ユーザーからの入力があった場合には、所定の補正条件の成立の如何に関わらず、補正部と状態推定部に、補正指示を行うことができ、この際に特定した方位角補正データも通知することができる。
【００２８】
また、本発明の他の態様では、前記状態推定装置を含むことを特徴とする電子機器に関係する。
【図面の簡単な説明】
【００２９】
【図１】比較例を説明するフローチャート。
【図２】図２（Ａ）〜図２（Ｄ）は、比較例の問題点の説明図。
【図３】本発明の概要を説明するフローチャート。
【図４】本実施形態の状態推定装置及びこれを含む電子機器の構成例。
【図５】偏角・伏角・全磁力の説明図。
【図６】図６（Ａ）、図６（Ｂ）は、補正条件の説明図。
【図７】図７（Ａ）、図７（Ｂ）は、全磁力に基づく補正条件の判定処理の説明図。
【図８】図８（Ａ）、図８（Ｂ）は、伏角に基づく補正条件の判定処理の説明図。
【図９】図９（Ａ）、図９（Ｂ）は、マップマッチング結果に基づく補正条件の判定の説明図。
【図１０】マップマッチング結果に基づき補正条件を判断する手法のフローチャート。
【図１１】全磁力に基づき補正条件を判断する手法のフローチャート。
【発明を実施するための形態】
【００３０】
以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。
【００３１】
１．概要
ここでは、地磁気センサーや加速度センサー等から取得した観測信号に基づいて、方位角を推定する慣性航法システムを実現する状態推定装置に関する実施形態について説明する。
【００３２】
慣性航法システムにおいては、地磁気センサー等から得られる観測信号を基に方位角を推定する。ただし、地磁気センサーから直接求められる方位角は、磁北を基準としたものである。そのため、地図等で一般的に用いられている真北を基準とした方位角に、観測信号から直接求めた方位角を補正する必要があり、偏角に基づきこの補正処理を行っている。
【００３３】
しかし、地磁気センサーは、地磁気環境の悪い場所では誤った方位角を検出してしまうことある。例えば、地磁気センサーを携帯電話やＰＣなどに近づけると、外部の磁場を検出してしまい、大きく誤った方角を検出してしまう。そのため、上記の補正処理によって求められる真北を基準とした方位角も誤った値となってしまう。この後、慣性航法システムでは、カルマンフィルターを用いて現在の方位角を濾波する処理が行われるが、カルマンフィルターの入力である真北を基準とした方位角が間違っている状態が続くと、その後に推定される方位角も期待値から大きく外れたものとなってしまう。
【００３４】
そこで、本実施形態の比較例として、図１のフローチャートに示すような手法が考えられる。この比較例では、まず地磁気センサーや加速度センサー等から観測信号を受信し（Ｓ１）、次に観測信号を観測量に変換する（Ｓ２）。そして、観測量に表される磁北を基準とした方位角を、真北を基準とした方位角に偏角を用いて補正する（Ｓ３）。ここで、今回検出した観測信号に基づく方位角が誤っていると判断する場合には、上記の補正処理に加えて、今回検出した観測信号に基づく方位角と、期待する方位角との差などに基づき、観測量を補正する（Ｓ３）。そして、カルマンフィルターによる状態推定を行い（Ｓ４）、カルマンフィルターの内部状態変数の更新を行う（Ｓ５）。そして、一定時間周期で、Ｓ１〜Ｓ５の処理を繰り返す。つまり、本比較例は、今回検出した観測信号に基づく方位角が誤っていると判断された若しくは判断した時に、上記手段により、入力に含まれる誤差を軽減するという手法である。
【００３５】
しかし、比較例のように、観測量のみを補正した場合において、地磁気センサーによって検出される地磁気に同程度の誤差が混じり続ける場合には、図２（Ａ）〜図２（Ｄ）に示すように濾波した結果が変化する。図２（Ａ）は、ｋ回目の状態推定において、実際の真北の方向がＮ軸方向であるが、真北の方向をＥＶ１と濾波し、Ｎ軸とＥＶ１の差分となる角度がθ１である場合を示している。次に、図２（Ｂ）は、ｋ＋１回目の状態推定において、前回の誤差θ１を用いて、入力値を補正した後の濾波結果を示している。図２（Ｂ）では、真北の方向をＥＶ２と濾波し、Ｎ軸とＥＶ２との差分となる角度がθ２となっており、濾波結果が期待値に近づいている。そして、図２（Ｃ）は、ｋ＋２回目の状態推定結果を示している。この時も図２（Ｂ）と同様に、θ１で観測量を補正しているにも関わらず、濾波した真北方向ＥＶ３とＮ軸の差がθ３となり、θ２よりも大きくなってしまっている。さらに、図２（Ｄ）では、ｋ＋３回目の状態推定を行っているが、濾波した真北方向ＥＶ４とＮ軸の差であるθ４がθ１とほぼ同じになってしまっている。つまり、比較例のように、観測量のみを補正した場合には、元の誤った方位角に濾波結果が次第に戻っていってしまうという問題が発生する。
【００３６】
比較例において、図２のように濾波結果が変化してしまう原因は、カルマンフィルターの内部状態変数の中に、前回の状態推定結果を示す値などが含まれているにも関わらず、観測量のみを補正しており、観測量と内部状態変数の整合性が取れなくなっていることであると考えられる。
【００３７】
そこで、この比較例の問題点に対して、本出願人は、検出される観測信号に誤差が含まれると判断される場合において、観測量の他に内部状態変数も補正する手法を提案する。
【００３８】
本手法の概要を図３のフローチャートに示す。観測信号の受信（Ｓ１０）、信号変換（Ｓ１１）、観測量の補正（Ｓ１２）は、比較例と同様である。これらの処理を行った後に、今回検出した観測信号に基づく方位角と期待する方位角との差分などに基づき、内部状態変数の補正を行う（Ｓ１３）。そして、カルマンフィルターによる状態推定（Ｓ１４）、内部状態変数の更新（Ｓ１５）を行う。
【００３９】
以下、本実施形態のシステム構成例を説明し、その後、フローチャートを用いて本実施形態における処理の詳細について説明する。
【００４０】
２．カルマンフィルターの原理
以下、カルマンフィルターの概要を簡単に説明する。
【００４１】
カルマンフィルターは、直接観測不可能な値を、観測可能な値を用いて精度良く推定する際に用いられる状態推定器である。
【００４２】
例えば、ある時刻ｔｋにおいて、あるシステムの観測不可能な状態量Ｘｋを求めたい場合を考える。Ｘｋは直接観測できない値であるが、逆にＸｋを用いれば、観測可能な値である観測量Ｚｋを、（１）式で表せることが分かっている。
【００４３】
【数１】
【００４４】
（１）式において、ｋはステップ数、Ｘｋは時刻ｔｋにおけるシステムの状態推定値、Ｚｋは時刻ｔｋにおいて入力される観測量、Ｈｋは観測行列、Ｖｋは観測雑音を示している。なお、Ｈｋは物理法則や測定等によって予め確定した行列である。（１）式を観測方程式という。
【００４５】
また、時刻ｔｋにおけるシステムの状態量Ｘｋは、１ステップ前の時刻ｔｋ-１の状態量Ｘｋ-１をもとに、次のように表現されることが分かっているとする。
【００４６】
【数２】
【００４７】
（２）式において、Ｘｋ-１は時刻ｔｋ-１におけるシステムの状態量、Ｆｋは状態遷移行列、Ｂｋは駆動行列、Ｕｋは制御入力、Ｗｋはシステム雑音（白色雑音）を示している。なお、Ｆｋ及びＢｋは、物理法則や測定等によって予め確定した行列である。（２）式を状態方程式という。
【００４８】
ここで、（１）式に観測により得られた観測量Ｚｋを代入すれば、真の状態量Ｘｋが求められるようにも思えるが、観測雑音Ｖｋが不明であるため、（１）式から直接に真の状態量Ｘｋを求めることはできない。
【００４９】
そこで、まず、（２）式において、システム雑音Ｗｋがないと仮定した場合を考え、その場合の（２）式に相当する（３）式により、状態推定値Ｘｋ|ｋ-１を求める。
【００５０】
【数３】
【００５１】
（３）式において、Ｘｋ|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値であり、Ｘｋ-１|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋ-１のシステムの状態推定値である。
【００５２】
次に、（１）式において、観測誤差がないと仮定した場合を考え、その場合の（１）式に相当する（４）式により、観測誤差がないと仮定した場合の観測量の予測値Ｚｋ，eを求める。（４）式のＸｋ|ｋ-１は（３）式により求められたものである。
【００５３】
【数４】
【００５４】
そして、実際に観測により得られた観測量Ｚｋ，ｒとＺｋ，eとの差をとる。
【００５５】
【数５】
【００５６】
Δｋを観測残差という。観測残差は、以下の３値に起因する値である。一つ目の値は、雑音がないとした場合における、状態推定値から求めた観測量の推定値（観測推定量）と、実際の観測量との誤差である。そして、二つ目の値は、観測雑音Ｖｋであり、三つ目の値は、システム雑音Ｗｋである。
【００５７】
さらに、観測残差Δｋの共分散Ｓｋを算出する。観測残差Δｋの共分散Ｓｋは、システムの状態推定値Ｘｋから求められた観測推定量の確からしさと、観測雑音とシステム雑音がどの程度含まれているかを表すものである。つまり、観測残差Δｋの共分散Ｓｋをできるだけ小さくすることができれば、観測雑音とシステム雑音を抑圧しつつ、観測推定量を確からしいものにすることができる。さらに、観測推定量の正確性が担保されれば、観測推定量を求める際に参照された状態推定値の正確さも期待できる。
【００５８】
したがって、観測残差Δｋの共分散Ｓｋをできるだけ小さくするように制御すれば、システムの状態推定値も確からしいものとなる。
【００５９】
カルマンフィルターでは、以上の原理を基に、観測更新と予測処理との２つの処理をステップ毎に繰り返して、尤もらしい状態推定値Ｘｋを推定する。これらの処理の詳細は、以下のシステム構成において述べる。
【００６０】
３．システム構成例
次に、図４に本実施形態の状態推定装置及びこれを含む電子機器の構成例を示す。
【００６１】
状態推定装置１００は、信号変換部１２０と、制御部１４０と、補正部１６０と、状態推定部１８０と、記憶部２００と、を含む。また、状態推定装置１００を含む電子機器の例としては、センサー１０や、外部制御部１１、操作部１２、表示部１４などを含む携帯端末などが挙げられる。なお、状態推定装置１００及びこれを含む電子機器は、図４の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００６２】
ここで、センサー１０は、地磁気センサー等の方位センサーや、加速度センサー、ジャイロセンサー等のセンサーであり、観測信号を信号変換部１２０に出力する。方位センサーは、例えば地磁気センサー等であり、センサーの向いている方位を角度（０°〜３６０°）で計測する。地磁気センサーは、例えば磁場の強さによって抵抗値やインピーダンス値が増減する素子等で構成され、三軸の地磁気情報を検知する。加速度センサーは、例えば外力によって抵抗値が増減する素子等で構成され、三軸の加速度情報を検知する。ジャイロセンサーは、三軸の角速度情報を検知する。また、地磁気センサーや加速度センサー、ジャイロセンサーの機能を併せ持つセンサーを用いてもよい。さらに、地磁気情報や加速度情報から、磁北を基準とした偏角を算出し、出力するセンサーを用いてもよい。
【００６３】
ここで、地磁気とは、地球が持つ磁気及びそれにより地球上に生じる磁場をいう。地磁気はベクトル量であり、大きさと方向を持つ。地磁気（地磁気ベクトル）の大きさは全磁力によって表され、地磁気の方向は偏角と伏角により表される。偏角、伏角、全磁力をまとめて、地磁気の３要素という。図５のＮＥＤ座標系においては、ＤＲＧが地磁気ベクトルを示している。
【００６４】
また、一般的に、方位角を求める際には、「北」を基準にするが、地磁気センサーから直接求められる「北」は磁北であり、真北とは異なる。
【００６５】
磁北とは、地磁気ベクトルを水平面へ正射影した正射影ベクトルの方向のことである。言い換えると、地磁気のＳ極に対して、方位磁石の磁針のＮ極が反応して指し示す方位のことである。図５では、ＤＲＭＮの方向が磁北である。
【００６６】
一方、真北とは、地軸の北極点を指す方向のことを言う。一般的に、地理上で「北」と言えば、真北のことを指す。図５では、Ｎ軸方向が真北である。
【００６７】
真北と磁北はずれており、磁北と真北とが形成する角のことを偏角という。例えば、東京付近では磁北は真北に対して西に約７度ずれている。右手系のＮＥＤ座標系では、西に傾いている場合にマイナス、東に傾いている場合にプラスとなるように定義されるため、前述した例で言えば、東京付近では磁北は真北に対して−７度ずれているという。偏角は地域により異なる。図５では、θＤが偏角である。
【００６８】
さらに、地磁気ベクトルは、赤道付近を除けば、地面に対して平行ではなく、地面と斜めに交わっている。地磁気ベクトルを垂直面へ正射影した正射影ベクトルの方向と、水平面とが形成する角のことを伏角という。右手系のＮＥＤ座標系では、地磁気が水平面に向かって突き刺さる方向の場合がプラス、水平面から出て行く向きの場合がマイナスとなるように定義される。伏角も地域により異なる。図５では、θＩが伏角である。
【００６９】
そして、地磁気の大きさを全磁力という。偏角と伏角同様、全磁力も場所によって異なる。図５では、地磁気ベクトルＤＲＧの長さとして表されるＴＭＦが全磁力となる。
【００７０】
外部制御部１１は、例えば、本実施形態の状態推定装置を搭載した携帯電子機器等に設けられ、後述する補正条件の判断を行ってもよく、方位角補正データを制御部１４０に通知してもよい。操作部１２は、例えば本実施形態の状態推定装置を搭載した携帯電子機器等に設けられるボタン等であり、表示部１４は携帯電子機器の画面等である。
【００７１】
次に各部で行われる処理について説明する。
【００７２】
信号変換部１２０は、センサー１０から観測信号を取得し、観測量に変換する。そして、観測量を制御部１４０と補正部１６０とに出力する。
【００７３】
ここで、観測信号とは、センサーから直接得られる情報をいう。例えば、本実施形態では、観測信号Ｙを（６）式のように設定する。
【００７４】
【数６】
【００７５】
（６）式において、θ’は磁北を基準とした方位角である。また、「（ａｘ，ａｙ，ａｚ）」はボディー座標系で加速度センサーが検出する重力加速度である。「（ωｘ，ωｙ，ωｚ）」はボディー座標系でジャイロセンサーが検出する角速度である。ただし、観測信号はここで説明したものに限定されない。例えば、ボディー座標系で地磁気センサーが検出する地磁気である「（ｇｘ，ｇｙ，ｇｚ）」などを観測信号に含んでも良い。
【００７６】
また、観測量とは、カルマンフィルターの入力となる情報をいう。具体例については後述する。
【００７７】
制御部１４０は、補正部１６０と状態推定部１８０の制御を行う。
【００７８】
補正部１６０は、信号変換部１２０から取得した観測量のうち磁北を基準とした方位角θ’を、偏角θＤに基づき（７）式にて補正し、真北を基準とする方位角θを算出する。
【００７９】
【数７】
【００８０】
状態推定部１８０は、補正部１６０から取得した補正後の観測量を入力として、カルマンフィルターにより、方位角を推定する。
【００８１】
ここで、本実施形態では、観測量Ｚを次式（８）のように設定する。
【００８２】
【数８】
【００８３】
（８）式において、θは（７）式により求められた方位角である。「（ａｘ，ａｙ，ａｚ）」は（６）式における重力加速度と同一の値である。「（ＶＮ，ＶＥ，ＶＤ）」はＮＥＤ座標系における速度の南北方向（Ｎ）、東西方向（Ｅ）及び高度方向（Ｄ）の各成分である。「（ωｘ，ωｙ，ωｚ）」は（６）式における角速度と同一の値である。ただし、観測量はここで説明したものに限定されない。
【００８４】
また、状態量とは、システムの観測対象であるが、直接は観測不可能な値をいい、状態推定値とは、カルマンフィルターにより状態量を推定した値をいう。例えば、本実施形態では、カルマンフィルターにおいて推定対象とするシステムの状態推定値Ｘを次式（９）のように設定する。
【００８５】
【数９】
【００８６】
（９）式において、「（ｑ０，ｑ１，ｑ２，ｑ３）」はクォータニオン（四元数）を表し、「（ｖＮ，ｖＥ，ｖＤ）」はＮＥＤ座標系における速度の南北方向（Ｎ）、東西方向（Ｅ）及び高度方向（Ｄ）の各成分である。
【００８７】
なお、ここで説明したものに限定されず、状態量としてクォータニオンの代わりにロール・ピッチ・ヨー角を用いる方法や、速度の他に、状態量に緯度・経度・高度を追加する方法、状態量にジャイロや加速度のバイアス値を追加する方法などが可能である。
【００８８】
以上の前提条件の下に、カルマンフィルターでは、観測更新と予測処理との２つの処理を繰り返し行う。観測更新とは、外部から与えられる（実際に観測した）観測量Ｚｋ，ｒを利用して１ステップ前に予測した状態推定値Ｘｋ｜ｋ-１及び誤差共分散Ｐｋ｜ｋ-１を更新する処理である。一方、予測処理とは、更新した状態推定値Ｘｋ｜ｋ及び誤差共分散Ｐｋ｜ｋから次のステップの状態推定値Ｘｋ+１｜ｋ及び誤差共分散Ｐｋ+１｜ｋを予測する処理である。そして、カルマンフィルターは、この２つの処理を繰り返すことにより、尤もらしい状態推定値Ｘｋを推定する。
【００８９】
まず、観測更新は、次式（１０）〜（１３）で定式化される。
【００９０】
【数１０】
【００９１】
【数１１】
【００９２】
【数１２】
【００９３】
【数１３】
【００９４】
（１０）〜（１３）において、Ｓｋは、上述した観測残差Δｋの共分散である。Ｐｋ｜ｋ-１は、時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値Ｘｋ｜ｋ−１と、時刻ｔｋの実際の状態量Ｘｋとの差の共分散（誤差共分散）である。なお、Ｉは単位行列である。
【００９５】
また、観測行列Ｈｋは、状態推定値Ｘｋの次元を観測量Ｚｋの次元に変換するための変換行列として用いられる。観測行列Ｈｋを１ステップ前に予測された状態推定値Ｘｋ｜ｋ-１に作用させることで、観測量の予測値Ｚｋ，eが得られる。
【００９６】
本実施形態において、観測行列Ｈｋは以下のようになる。まず、θについては、（１４）式にて表すことができる。
【００９７】
【数１４】
【００９８】
次に、「（ａｘ，ａｙ，ａｚ）」については、（１５）式にて表すことができる。
【００９９】
【数１５】
【０１００】
そして、「（ｖＮ，ｖＥ，ｖＤ）」については、（１６）式にて表すことができる。
【０１０１】
【数１６】
【０１０２】
また、式（１１）において、Ｋｋはカルマンゲインと呼ばれる補正係数である。カルマンゲインＫｋは、予測された状態推定値Ｘｋ｜ｋ-１を重視するか、Ｚｋ，ｒとＺｋ，eとの差Δｋを重視するかを決定付けるパラメーター値として用いられる。カルマンゲインＫｋは、観測誤差Ｒｋの大きさによって決定付けられる。観測誤差Ｒｋは、観測量Ｚｋに含まれる想定誤差の大きさを表すパラメーター（想定誤差パラメーター）である。観測誤差Ｒｋは、期待値（平均値）を“０”とする正規分布型のホワイトノイズと仮定して設定される。
【０１０３】
式（１２）は、Ｚｋ，ｒとＺｋ，eとの差Δｋを用いて、状態推定値Ｘｋ｜ｋを更新することを表している。
【０１０４】
また、式（１３）は、１ステップ前に予測された誤差共分散Ｐｋ｜ｋ-１とカルマンゲインＫｋと観測行列Ｈｋとを用いて、誤差共分散Ｐｋ｜ｋを更新することを表している。
【０１０５】
一方、予測処理は、次式（１７）及び（１８）で定式化される。
【０１０６】
【数１７】
【０１０７】
【数１８】
【０１０８】
本実施例においては、ＢｋとＵｋは考慮しなくてもよい。また、（１８）式において、Ｑｋはシステム雑音と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。
【０１０９】
ここで、状態遷移行列Ｆｋは、観測更新で更新された状態推定値Ｘｋ|ｋから次のステップの状態推定値Ｘｋ+１|ｋへの遷移を表す行列として定義される。
【０１１０】
本実施形態における状態遷移行列Ｆｋは以下のようになる。まず、「（ｑ０，ｑ１，ｑ２，ｑ３）」については、（１９）式にて表すことができる。
【０１１１】
【数１９】
【０１１２】
次に、「（ｖＮ，ｖＥ，ｖＤ）」については、（２０）式にて表すことができる。
【０１１３】
【数２０】
【０１１４】
なお、（１９）式、（２０）式において、Δｔはサンプリング間隔である。例えば５０Ｈｚでサンプリングされる場合、Δｔ＝１／５０［ｓｅｃ］である。
【０１１５】
また、（３）式〜（５）式及び（１０）式〜（２０）式にて、表される値等をまとめて、カルマンフィルターの内部状態変数と呼ぶ。
【０１１６】
記憶部２００は、種々の情報を記憶するとともに、各部のワーク領域となるものであり、その機能はＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現できる。
【０１１７】
以上の本実施形態では、観測により入力される観測信号に基づいて、観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部１８０と、観測信号を観測量に変換し、観測量を補正部１６０に出力する信号変換部１２０と、観測量の補正処理を行い、補正処理後の観測量を状態推定部１８０に出力する補正部１６０と、補正部１６０と状態推定部１８０を制御する制御部１４０と、を含む。そして、制御部１４０は、所定の補正条件が成立したと判断する場合、又は所定の補正条件が成立した旨を通知された場合に、補正部１６０及び状態推定部１８０に補正指示を行い、補正部１６０は、補正指示が行われた場合に、観測量の補正処理を行い、状態推定部１８０は、補正指示が行われた場合に、観測量に対応する内部状態変数の補正処理を行い、補正後の観測量と内部状態変数とに基づいて状態推定を行う。
【０１１８】
これにより、まず、入力された観測信号を観測量へ変換することができる。次に、補正条件が成立したと判断する場合、又は補正条件が成立した旨を他機能部や外部制御部１１から通知された場合に、観測量と内部状態変数の補正指示をし、これらの値を補正することができる。そして、補正した観測量を入力とし、内部状態変数を補正した状態で、カルマンフィルターにより、状態推定を行うことができる。
【０１１９】
ここで、補正条件とは、補正部１６０にて行う補正処理を決定付けるために設けられた判断基準である。詳しくは具体例とともに後述する。
【０１２０】
また、補正指示とは、補正条件が成立したことを通知するために、制御部１４０が補正部１６０及び状態推定部１８０に対して行う指示のことである。例えば、補正指示の一例として、補正条件成立時に、制御部１４０が補正部１６０及び状態推定部１８０に補正指示情報を出力することなどがある。なお、補正指示は、様々な変形が可能であり、これに限定されるものではない。例えば、補正条件成立時にのみ補正指示情報を出力するだけではなく、不成立時にも出力するが、補正指示情報に含まれる内容により、補正条件を満たしたか否かということが分かる方法でも良い。逆に、補正条件を満たさない時にのみ、制御部１４０が補正部１６０及び状態推定部１８０に補正指示情報を出力して、補正条件の成否を判断しても良い。他にも例えば、後述する方位角補正データの通知もって補正指示情報の通知としてもよい。
【０１２１】
以上より、センサー１０により検出された観測信号に基づく方位角（検出値）と期待する方位角（期待値）との誤差が大きい時に補正条件を満たすように設定をしておけば、検出値が誤っている場合等に、観測量だけでなく、内部状態変数のうち、前回推定した状態推定値も補正した状態で、新たに状態推定を行うことができるようになる。
【０１２２】
式（１２）に示すように、カルマンフィルターでは、前回推定した状態推定値を用いて、漸化式により現在の状態を濾波する。すなわち、前回推定した状態推定値の誤差が大きければ、現在の状態推定値の誤差も大きくなり、前回推定した状態推定値の誤差が小さければ、現在の状態推定値の誤差も小さくなる。例えば、上記で述べた比較例のように、観測量のみを補正した場合には、前回の状態推定値の誤差が補正されておらず、現在の状態推定値がその誤差の影響を受けてしまい、回を重ねる毎に誤差が蓄積され、元の誤った状態推定を行うようになってしまう。一方、本実施形態では、この前回の状態推定値の誤差を補正によって取り除き、前回の状態推定値を正しい値に書き換えているため、現在の状態推定において、前回までの状態推定の結果が反映されにくくなる。
【０１２３】
よって、観測量だけを補正して、状態推定を行う場合と比べて、補正処理後に状態推定を繰り返しても、状態推定値が補正前の値に戻りにくくすることができる。
【０１２４】
また、信号変換部１２０は、方位センサーからのセンサー信号を含む観測信号を、磁北を基準とする方位角の情報を含む観測量に変換してもよい。制御部１４０は、所定の補正条件が成立したと判断する場合、又は所定の補正条件が成立した旨を通知された場合に、補正部１６０及び状態推定部１８０に補正指示を行い、方位角補正データを通知してもよい。補正部１６０は、補正指示が行われた場合に、制御部１４０から通知された方位角補正データと、磁北を真北に補正する偏角とに基づいて、観測量の補正処理を行い、補正後の観測量を状態推定部１８０に出力してもよい。状態推定部１８０は、補正指示が行われた場合に、方位角補正データに基づいて、観測量に対応する内部状態変数の補正処理を行い、補正後の観測量と内部状態変数とに基づいて状態推定を行ってもよい。
【０１２５】
これにより、方位センサーからのセンサー信号を含む観測信号を、方位角の情報を含む観測量に変換することができる。そして、方位角の情報を含む観測量と内部状態変数を補正しながら、状態推定を行うことができるようになる。
【０１２６】
方位角補正データとは、センサー１０が検出した観測信号に基づく方位角（検出値）を、正しい方位角に補正するための情報である。例えば、検出値が磁北に対して−１０度（すなわち、右手系のＮＥＤ座標系で磁北に対して西に１０度傾いた方向）であった時に、外部制御部により正しい方位角が−７度であると判断された場合には、＋３度が方位角補正データとなる。なお、この例では角度で示したが、クォータニオン等により方位角補正データを表すことも当然可能である。また、真北を基準として、方位角補正データを算出してもよい。
【０１２７】
よって、本実施形態によれば、カルマンフィルターにより、方位角を推定しつつ、その推定した方位角が誤っている場合でも、補正処理により推定誤差をキャンセルし、ノイズを抑圧して、正しい方位角を推定することが可能となる。
【０１２８】
また、補正部１６０は、補正指示が行われた場合には、方位角補正データと偏角とに基づいて、観測量の補正処理を行い、補正指示が行われない場合には、偏角に基づいて、観測量の補正処理を行ってもよい。
【０１２９】
これにより、補正指示が行われた場合には、方位角補正データと偏角とに基づいて観測量を補正し、補正指示が行われない場合には、偏角によって観測量を補正して、状態推定を行うことができる。すなわち、本実施形態によれば、所定の補正条件が成立するか否かによって、観測量を補正する際に用いる値を変えることが可能となる。
【０１３０】
そのため、地磁気環境が悪い場合に、補正条件を満たすと判断する、又は判断されるようにしておけば、地磁気環境が悪い場合にのみ、方位角補正データと偏角とによって観測量を補正し、地磁気環境が良い場合には、通常用いる偏角のみを用いて観測量を補正することができるようになる。
【０１３１】
具体例を図６（Ａ）（Ｂ）に示す。図６（Ａ）（Ｂ）は、本実施形態が携帯端末ＤＶにて実現されており、ＤＶで方角推定を行う様子を示している。図６（Ａ）のように屋外で見晴らしが良く、周囲に地磁気に影響を及ぼすものがないような地磁気環境が良い場所においては、通常用いる偏角のみを用いて、観測量を補正する。これは、地磁気環境の悪い時に与えられた方位角補正データを補正処理に用いることにより、観測量に余計な修正を加える結果となってしまい、かえって状態推定値が期待値から離れてしまうことを避けるためである。
【０１３２】
一方、図６（Ｂ）に示すように、屋内等で周囲に電化製品等が沢山あるような地磁気環境の悪い場所では、方位角補正データと偏角を用いて、観測量を補正する。この場合には、通常用いる偏角のみで補正処理を行ったとしても、観測量に多く含まれる誤差を除去できないためである。
【０１３３】
したがって、補正条件の設定に応じて、適切な補正処理を行うことが可能となる。
【０１３４】
また、状態推定部１８０は、補正指示が行われた場合には、方位角補正データに基づいて、内部状態変数の補正処理を行い、補正指示が行われない場合には、内部状態変数の補正処理を行わなくてもよい。
【０１３５】
これにより、補正指示が行われない場合に、前回更新した値に補正を加えていない内部状態変数と、偏角により補正された観測量とを用いて、状態推定を行うこと等が可能となる。すなわち、この場合には、観測量も内部状態変数も方位角補正データによる補正を行っていないこととなる。よって、両変数の整合性を図ることができる。その結果、補正条件を満たさない場合、例えば図６（Ａ）のような地磁気環境が良い場合にも、ノイズを抑圧して、正しい方位角を推定することが可能となる。
【０１３６】
これまでに述べてきた方法により、所定の補正条件の判断により、異なる動作をする補正処理を行うことができる。以下では、補正条件の判断方法について述べる。
【０１３７】
地磁気センサーを用いて方位角を推定する場合、方位角の推定結果は地磁気の環境に大きな影響を受ける。すなわち、地磁気環境が良い場合と悪い場合に場合分けをして、異なる補正処理を行うことが好ましい。したがって、本実施形態では以下で述べる方法で地磁気環境の良し悪しを判断する。
【０１３８】
信号変換部１２０は、方位センサーから通知された観測信号に基づいて、全磁力を算出し、全磁力を制御部１４０に通知し、制御部１４０は、通知された全磁力に基づいて、所定の補正条件の判断をし、全磁力が所定の閾値により特定される範囲内にない場合には、所定の補正条件を満たすと判断し、全磁力が所定の閾値の範囲内にある場合には、所定の補正条件を満たさないと判断してもよい。
【０１３９】
これにより、方位センサーにより検出された観測信号に基づき、全磁力を算出することができ、その全磁力に基づいて、補正処理を行うか否かを決定すること等ができる。
【０１４０】
本来、地磁気が正しく検出された場合には、地磁気の強さを示す全磁力も全磁力の期待値に近い値になるはずである。すなわち、検出された全磁力（サンプル値）と全磁力の期待値とを比較し、サンプル値が期待値と十分に近いかどうかを判断することで、地磁気が正しく検出されたかどうかを判断することができる。さらに、地磁気が正しく検出された場合には、地磁気環境が良く、地磁気が正しく検出されなかった場合には地磁気環境が悪いと判断する。
【０１４１】
具体例を図７（Ａ）（Ｂ）に示す。図７（Ａ）（Ｂ）は、ＮＥＤ座標系において、実際の地磁気の方向（期待値）がＤＲＧである場合を示す。ここでは、観測信号に基づき、ＤＲＯ１及びＤＲＯ２、ＤＲＯ３に示す地磁気が導かれた場合を考える。図７（Ａ）に示すように、各地磁気の偏角はθＤで、同一であり、各地磁気の矢印の長さがそれぞれの全磁力を表している。
【０１４２】
図７（Ｂ）は、図７（Ａ）の地磁気の全磁力を縦軸とするグラフであり、ＤＲＧの全磁力をＴＭＦ、ＤＲＯ１〜ＤＲＯ３の全磁力をそれぞれＴＭＦ１〜ＴＭＦ３として示す。
【０１４３】
ここでは、全磁力のサンプル値と期待値が十分に近いかどうかを判断するために、任意の全磁力の閾値ＴｈｍａｘとＴｈｍｉｎを設定し、これらの閾値に挟まれる範囲ΔＦを設定する。サンプル値がΔＦ内に含まれれば、期待値ＴＭＦに近いと判断し、地磁気環境が良いと判断する。逆に、サンプル値がΔＦ内に含まれなければ、期待値ＴＭＦからかけ離れていると判断し、地磁気環境が悪いと判断する。図７（Ｂ）の例においては、ＴＭＦ２のみがΔＦ内に含まれているため、ＤＲＯ２の地磁気環境は良いと判断される。逆に、ＴＭＦ１とＴＭＦ３はどちらもΔＦ内に含まれないため、ＤＲＯ１とＤＯＲ３の地磁気環境は悪いと判断される。
【０１４４】
このように、本実施形態では、全磁力を用いることで、地磁気環境が良いか悪いかを判定して、補正処理を行うか否かを決定すること等ができる。
【０１４５】
また、信号変換部１２０は、方位センサーから通知された観測信号に基づいて、伏角を算出し、伏角を制御部１４０に通知し、制御部１４０は、通知された伏角に基づいて、所定の補正条件の判断をし、伏角が所定の閾値により特定される範囲内にない場合には、所定の補正条件を満たすと判断し、伏角が所定の閾値の範囲内にある場合には、所定の補正条件を満たさないと判断してもよい。
【０１４６】
これにより、方位センサーにより検出された観測信号に基づき、伏角を算出することができ、その伏角に基づいて、補正処理を行うか否かを決定すること等ができる。
【０１４７】
全磁力を使って、補正条件を判断する場合と同様に、本来、地磁気が正しく検出された場合には、伏角も伏角の期待値に近い値になるはずである。すなわち、検出された伏角のサンプル値と伏角の期待値とを比較し、サンプル値が期待値と十分に近いかどうかを判断することで、地磁気が正しく検出されたかどうかを判断することができる。
【０１４８】
具体例を図８（Ａ）（Ｂ）に示す。図８（Ａ）（Ｂ）は、ＮＥＤ座標系において、実際の地磁気の方向（期待値）がＤＲＧである場合を示す。ここでは、観測信号に基づき、ＤＲＯ４及びＤＲＯ５、ＤＲＯ６に示す地磁気が導かれた場合を考える。図８（Ａ）に示すように、各地磁気の偏角はθＤで、同一であり、Ｎ−Ｅ平面（Ｎ軸とＥ軸を含む平面）に対する各地磁気の矢印の角度がそれぞれの伏角を表している。
【０１４９】
図８（Ｂ）は、図８（Ａ）の地磁気の伏角を表した、Ｄ軸とＧ軸からなる二次元グラフであり、ＤＲＧの伏角をθＩ、ＤＲＯ４〜ＤＲＯ６の伏角をそれぞれθＯ４〜θＯ６として示す。
【０１５０】
ここでは、伏角のサンプル値と期待値が十分に近いかどうかを判断するために、ＤＲＧ方向を中心とする任意の角度範囲Δθを設定する。サンプル値がΔθ内に含まれれば、期待値θＩに近いと判断し、地磁気環境が良いと判断する。逆に、サンプル値がΔθ内に含まれなければ、期待値θＩからかけ離れていると判断し、地磁気環境が悪いと判断する。図８（Ｂ）の例においては、θＯ５のみがΔθ内に含まれているため、ＤＲＯ５の地磁気環境は良いと判断される。逆に、θＯ４とθＯ６はどちらもΔθ内に含まれないため、θＯ４とθＯ６の地磁気環境は悪いと判断される。
【０１５１】
このように、本実施形態では、伏角を用いることで、地磁気環境が良いか悪いかを判定して、補正処理を行うか否かを決定すること等ができる。
【０１５２】
また、制御部１４０は、信号変換部１２０から取得した観測量をマップマッチング部１６に出力し、マップマッチング部１６からマップマッチングの結果を取得し、マップマッチングの結果に基づいて、所定の補正条件を判断してもよい。
【０１５３】
これにより、マップマッチングの結果に基づいて、補正処理を行うか否かを決定すること等ができる。
【０１５４】
ここで、マップマッチングの結果とは、センサー１０が検出した観測信号に基づいて算出された方位角（検出値）と、方位角の期待値とを、外部に設けられたマップマッチング部１６が比較した結果である。マップマッチングの結果には、検出値と期待値が一致しているか否かといった情報や、検出値と期待値の差等が含まれる。
【０１５５】
具体例を図９（Ａ）（Ｂ）に示す。図９（Ａ）は、マップマッチングの結果を使って補正条件を判定する実施形態のシステム構成の一例である。図４に示すブロック図に加えて、本実施形態の状態推定装置１００の外部にマップマッチング部１６があり、制御部１４０はマップマッチング部１６から通知されるマップマッチングの結果を取得して、補正条件の判定を行う。
【０１５６】
図９（Ｂ）には、ユーザーＵＳが持つ端末が本実施形態を実現しており、図に示す位置において、方位角を検出し、マップマッチングを行う様子を示している。この場合に、例えば、ＤＲＥ方向が真北であるとユーザーＵＳの端末が検出したとする。すると、マップマッチング部１６は、実際の真北の方角ＤＲＮと、検出した方角ＤＲＥとを比較し、一致しているかどうかを判断し、その結果を制御部１４０に通知する。制御部１４０は、マップマッチングの結果、方角が一致していると判定された場合には、補正指示を行わないと判断し、方角が一致していないと判定された場合には、補正指示を行うと判断し、補正部１６０と状態推定部１８０に補正指示を通知する。
【０１５７】
以上のように、本実施形態では、マップマッチングの結果を用い、現在検出した方位角が正しいか否かを判定することにより、地磁気環境が良いか悪いかを判定して、補正処理を行うか否かを決定すること等ができる。
【０１５８】
また、上記で述べた補正条件が成立しない場合でも、推定した方位角が間違っていると感じるユーザーもいる。
【０１５９】
そこで、制御部１４０は、操作部１２から入力されたユーザー入力情報に基づいて、方位角補正データを特定し、補正部１６０及び状態推定部１８０に補正指示と方位角補正データとを通知し、補正部１６０は、補正指示が行われた場合には、方位角補正データと、偏角とに基づいて、観測量の補正処理を行い、状態推定部１８０は、補正指示が行われた場合には、方位角補正データに基づいて、内部状態変数の補正処理を行ってもよい。
【０１６０】
これにより、ユーザー入力情報に基づいて、方位角補正データを特定することができる。そして、ユーザーからの入力があった場合には、所定の補正条件の成立の如何に関わらず、補正部１６０と状態推定部１８０に、補正指示を行うことができ、この際に特定した方位角補正データも通知することができる。
【０１６１】
ここで、ユーザー入力情報とは、ユーザーが状態推定部１８０の出力として期待する方位角と、実際に状態推定部１８０が推定した方位角との差の情報を含む情報であり、操作部１２を通じてユーザーにより入力される情報である。
【０１６２】
以上より、推定した方位角が間違っているとユーザーが感じた場合にも、ユーザーが補正データを入力すること等によって、本実施形態の状態推定装置に補正指示を出すことを促すことができる。その結果、ユーザーのニーズに応じた正確性を有する方位角の推定を行うことができる。
【０１６３】
なお本実施形態の状態推定装置等は、プログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態の状態推定装置等が実現される。具体的には情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そしてＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。
【０１６４】
４．処理の流れ
以下では、図１０及び図１１のフローチャートを用いて、本実施形態の処理の流れについて説明する。
【０１６５】
４．１． マップマッチング結果に基づき補正条件を判断する手法の詳細
まず、信号変換部１２０が、センサー１０の地磁気センサー及び加速度センサー、ジャイロセンサー等からそれぞれ三軸の観測信号を取得する（Ｓ２０）。次に、信号変換部１２０は、観測信号に基づいて、磁北を基準とした方位角の情報を含む観測量を算出し（Ｓ２１）、制御部１４０と補正部１６０に観測量を出力する。そして、制御部１４０は、観測量に表される方位角をマップマッチング部１６に出力する（Ｓ２２）。その後、制御部１４０は、マップマッチング部１６から補正指示があるか否かを判断する（Ｓ２３）。制御部１４０は、マップマッチング部１６から補正指示を受けたと判断する場合には、補正部１６０と状態推定部１８０に補正指示をし、さらに、マップマッチング部１６から取得した方位角補正データを通知する。そして、補正部１６０は、方位角補正データと偏角とに基づき、観測量を補正し、状態推定部１８０は、方位角補正データに基づき内部状態変数を補正する（Ｓ２４）。一方、制御部１４０は、マップマッチング部１６から補正指示を受けていないと判断する場合には、補正指示を行わず、補正部１６０は、偏角に基づき、観測量を補正する（Ｓ２５）。観測量の補正処理が終わると、補正処理後の観測量を入力として、カルマンフィルターにより状態推定を行う（Ｓ２６）。そして、最後に、カルマンフィルターの内部状態変数の更新を行う（Ｓ２７）。
【０１６６】
本実施形態では、以上の処理を一定時間周期で行う。なお、マップマッチング部に方位角を通知し、マップマッチングを行う処理は毎回行う必要はない。この場合においては、マップマッチングを行わない場合には、前回マップマッチングを行った時と同様の補正指示と方位角補正データに基づいて補正処理を行ってもよい。
【０１６７】
４．２． 全磁力に基づき補正条件を判断する手法の詳細
まず、信号変換部１２０が、センサー１０の地磁気センサー及び加速度センサー、ジャイロセンサー等からそれぞれ三軸の観測信号を取得する（Ｓ３０）。次に、信号変換部１２０は、観測信号に基づいて、磁北を基準とした方位角と全磁力との情報を含む観測量を算出し（Ｓ３１）、制御部１４０と補正部１６０に観測量を出力する。そして、制御部１４０は、通知された全磁力が所定の範囲内である否かを判断する（Ｓ３２）。制御部１４０は、全磁力が所定の範囲内にないと判断した場合には、その旨を外部制御部に通知する（Ｓ３３）。制御部１４０が、全磁力が所定の範囲内にあると判断した場合、又はＳ３３の処理後において、制御部１４０は、外部制御部からの補正指示があるか否かを判断する（Ｓ３４）。制御部１４０は、外部制御部１１から補正指示を受けたと判断する場合には、補正部１６０と状態推定部１８０に補正指示をし、さらに、外部制御部１１から取得した方位角補正データを通知する。そして、補正部１６０は、方位角補正データと偏角とに基づき、観測量を補正し、状態推定部１８０は、方位角補正データに基づき内部状態変数を補正する（Ｓ３５）。一方、制御部１４０は、外部制御部１１から補正指示を受けていないと判断する場合には、補正指示を行わず、補正部１６０は、偏角に基づき、観測量を補正する（Ｓ３６）。観測量の補正処理が終わると、補正処理後の観測量を入力として、カルマンフィルターにより状態推定を行う（Ｓ３７）。そして、最後に、カルマンフィルターの内部状態変数の更新を行う（Ｓ３８）。
【０１６８】
本実施形態では、以上の処理を一定時間周期で行う。なお、制御部１４０による、全磁力が所定の範囲内にあるか否かを判断する処理は毎回行う必要はない。この場合においては、上記処理を行わない場合には、前回上記処理を行った時と同様の補正指示と方位角補正データに基づいて補正処理を行ってもよい。
【０１６９】
以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また状態推定装置及びこれを含む電子機器の構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１７０】
１０ センサー、１１ 外部制御部、１２ 操作部、１４ 表示部、
１６ マップマッチング部、１００ 状態推定装置、１２０ 信号変換部、
１４０ 制御部、１６０ 補正部、１８０ 状態推定部、２００ 記憶部
【技術分野】
【０００１】
本発明は、状態推定装置、電子機器及びプログラム等に関係する。
【背景技術】
【０００２】
近年、方角や位置等を求めるシステムとして、測位システムや慣性航法システムが広く知られている。
【０００３】
測位システムとしては、ＧＰＳ（Global Positioning System）が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。ＧＰＳでは、複数のＧＰＳ衛星の位置や、各ＧＰＳ衛星から位置算出装置までの擬似距離等の情報に基づいて、位置算出装置の位置等を算出する。また、位置算出装置の移動前の位置と移動後の位置とのベクトルを求めることにより、位置算出装置の移動している方角を推定することもできる。
【０００４】
一方、慣性航法システムは、外部からの電波による支援を得ることなく、自らに搭載する地磁気センサーや、ジャイロセンサー、加速度センサー等からの情報のみによって、自らの位置や速度、姿勢、向き等を推定する。地球上での絶対位置が分かるＧＰＳに対して、慣性航法システムは初期位置からの相対位置を推定することができる。
【０００５】
最近では、ＧＰＳや慣性航法システムを利用して、ユーザーの道案内をするサービス等も行われている。
【０００６】
これらのＧＰＳや慣性航法システムに関する発明としては特許文献１、２に記載される従来技術がある。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００６−１２６１４８号公報
【特許文献２】特開２００７−２７６５０７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
ＧＰＳは、電波状況に依存するため、地下や建物の中などでは位置や方角を正確に推定することができない。そのため、先に述べたＧＰＳを利用する道案内サービスでは、ユーザーが地下や建物の中に入ってしまうと、十分な精度を保ちつつ、サービスを提供することができないという点が問題となる。
【０００９】
一方、慣性航法システムを利用した道案内サービスは、電波状況には依存しないため、地下街等においても、精度を保ったまま道案内サービスを提供することができ、上記の問題を解決することができる。
【００１０】
しかし、慣性航法システムは、地磁気センサーを用いて、方角を推定するため、地磁気環境の悪い場所では誤った方角を推定してしまうという問題がある。例えば、地磁気センサーを携帯電話やＰＣなどに近づけると、外部の磁場を検出してしまい、大きく誤った方角を検出してしまう。
【００１１】
本発明の幾つかの態様によれば、観測量と、カルマンフィルターの内部状態変数との補正処理を行うことにより、誤った推定をした後も、元の推定精度を維持することができる状態推定装置、電子機器及びプログラム等を提供できる。
【課題を解決するための手段】
【００１２】
本発明の一態様は、観測により入力される観測信号に基づいて、前記観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部と、前記観測信号を観測量に変換し、前記観測量を補正部に出力する信号変換部と、前記観測量の補正処理を行い、補正処理後の前記観測量を前記状態推定部に出力する補正部と、前記補正部と前記状態推定部を制御する制御部と、を含み、前記制御部は、所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、前記補正部は、前記補正指示が行われた場合に、前記観測量の補正処理を行い、前記状態推定部は、前記補正指示が行われた場合に、前記観測量に対応する内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とする状態推定装置に関係する。また本発明の他の態様は、上記各部としてコンピューターを機能させるプログラム、又は該プログラムを記憶したコンピューター読み取り可能な情報記憶媒体に関係する。
【００１３】
本発明の一態様では、入力された観測信号を観測量へ変換する。次に、本発明の一態様では、補正条件が成立したと判断する場合、又は補正条件が成立した旨を通知された場合に、観測量と内部状態変数の補正指示が行われ、これらの値を補正する。これにより、補正した観測量を入力とし、内部状態変数を補正した状態で、カルマンフィルターにより、状態推定を行うことができる。
【００１４】
また、本発明の一態様では、前記信号変換部は、方位センサーからのセンサー信号を含む観測信号を、磁北を基準とする方位角の情報を含む観測量に変換し、前記制御部は、前記所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、方位角補正データを通知し、前記補正部は、前記補正指示が行われた場合に、前記制御部から通知された前記方位角補正データと、磁北を真北に補正する偏角とに基づいて、前記観測量の補正処理を行い、補正後の前記観測量を前記状態推定部に出力し、前記状態推定部は、前記補正指示が行われた場合に、前記方位角補正データに基づいて、前記観測量に対応する前記内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行ってもよい。
【００１５】
これにより、方位センサーからのセンサー信号を含む観測信号を、方位角の情報を含む観測量に変換することができる。そして、方位角の情報を含む観測量と内部状態変数を補正しながら、状態推定を行うことが可能になる。
【００１６】
また、本発明の一態様では、前記補正部は、前記補正指示が行われた場合には、前記方位角補正データと前記偏角とに基づいて、前記観測量の補正処理を行い、前記補正指示が行われない場合には、前記偏角に基づいて、前記観測量の補正処理を行ってもよい。
【００１７】
これにより、補正指示が行われた場合には、方位角補正データと偏角とに基づいて観測量を補正し、補正指示が行われない場合には、偏角によって観測量を補正して、状態推定を行うことができる。
【００１８】
また、本発明の一態様では、前記状態推定部は、前記補正指示が行われた場合には、前記方位角補正データに基づいて、前記内部状態変数の補正処理を行い、前記補正指示が行われない場合には、前記内部状態変数の補正処理を行わなくてもよい。
【００１９】
これにより、補正指示が行われない場合に、前回更新した値に補正を加えていない内部状態変数と、偏角により補正された観測量とを用いて、状態推定を行うこと等が可能となる。
【００２０】
また、本発明の一態様では、前記信号変換部は、方位センサーから通知された観測信号に基づいて、全磁力を算出し、前記全磁力を前記制御部に通知し、前記制御部は、通知された前記全磁力に基づいて、前記所定の補正条件の判断をし、前記全磁力が所定の閾値により特定される範囲内にない場合には、所定の補正条件を満たすと判断し、前記全磁力が前記所定の閾値の範囲内にある場合には、所定の補正条件を満たさないと判断してもよい。
【００２１】
これにより、方位センサーにより検出された観測信号に基づき、全磁力を算出することができ、その全磁力に基づいて、補正処理を行うか否かを決定すること等ができる。
【００２２】
また、本発明の一態様では、前記信号変換部は、前記方位センサーから通知された観測信号に基づいて、伏角を算出し、前記伏角を前記制御部に通知し、前記制御部は、通知された前記伏角に基づいて、前記所定の補正条件の判断をし、前記伏角が所定の閾値により特定される範囲内にない場合には、前記所定の補正条件を満たすと判断し、前記伏角が前記所定の閾値の範囲内にある場合には、前記所定の補正条件を満たさないと判断してもよい。
【００２３】
これにより、方位センサーにより検出された観測信号に基づき、伏角を算出することができ、その伏角に基づいて、補正処理を行うか否かを決定すること等ができる。
【００２４】
また、本発明の一態様では、前記制御部は、前記信号変換部から取得した前記観測量をマップマッチング部に出力し、マップマッチング部からマップマッチングの結果を取得し、前記マップマッチングの結果に基づいて、前記所定の補正条件を判断してもよい。
【００２５】
これにより、マップマッチングの結果に基づいて、補正処理を行うか否かを決定すること等ができる。
【００２６】
また、本発明の一態様では、前記制御部は、操作部から入力されたユーザー入力情報に基づいて、前記方位角補正データを特定し、前記補正部及び前記状態推定部に前記補正指示と前記方位角補正データとを通知し、前記補正部は、前記補正指示が行われた場合には、前記方位角補正データと、前記偏角とに基づいて、前記観測量の補正処理を行い、前記状態推定部は、前記補正指示が行われた場合には、前記方位角補正データに基づいて、前記内部状態変数の補正処理を行ってもよい。
【００２７】
これにより、ユーザー入力情報に基づいて、方位角補正データを特定することができる。そして、ユーザーからの入力があった場合には、所定の補正条件の成立の如何に関わらず、補正部と状態推定部に、補正指示を行うことができ、この際に特定した方位角補正データも通知することができる。
【００２８】
また、本発明の他の態様では、前記状態推定装置を含むことを特徴とする電子機器に関係する。
【図面の簡単な説明】
【００２９】
【図１】比較例を説明するフローチャート。
【図２】図２（Ａ）〜図２（Ｄ）は、比較例の問題点の説明図。
【図３】本発明の概要を説明するフローチャート。
【図４】本実施形態の状態推定装置及びこれを含む電子機器の構成例。
【図５】偏角・伏角・全磁力の説明図。
【図６】図６（Ａ）、図６（Ｂ）は、補正条件の説明図。
【図７】図７（Ａ）、図７（Ｂ）は、全磁力に基づく補正条件の判定処理の説明図。
【図８】図８（Ａ）、図８（Ｂ）は、伏角に基づく補正条件の判定処理の説明図。
【図９】図９（Ａ）、図９（Ｂ）は、マップマッチング結果に基づく補正条件の判定の説明図。
【図１０】マップマッチング結果に基づき補正条件を判断する手法のフローチャート。
【図１１】全磁力に基づき補正条件を判断する手法のフローチャート。
【発明を実施するための形態】
【００３０】
以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。
【００３１】
１．概要
ここでは、地磁気センサーや加速度センサー等から取得した観測信号に基づいて、方位角を推定する慣性航法システムを実現する状態推定装置に関する実施形態について説明する。
【００３２】
慣性航法システムにおいては、地磁気センサー等から得られる観測信号を基に方位角を推定する。ただし、地磁気センサーから直接求められる方位角は、磁北を基準としたものである。そのため、地図等で一般的に用いられている真北を基準とした方位角に、観測信号から直接求めた方位角を補正する必要があり、偏角に基づきこの補正処理を行っている。
【００３３】
しかし、地磁気センサーは、地磁気環境の悪い場所では誤った方位角を検出してしまうことある。例えば、地磁気センサーを携帯電話やＰＣなどに近づけると、外部の磁場を検出してしまい、大きく誤った方角を検出してしまう。そのため、上記の補正処理によって求められる真北を基準とした方位角も誤った値となってしまう。この後、慣性航法システムでは、カルマンフィルターを用いて現在の方位角を濾波する処理が行われるが、カルマンフィルターの入力である真北を基準とした方位角が間違っている状態が続くと、その後に推定される方位角も期待値から大きく外れたものとなってしまう。
【００３４】
そこで、本実施形態の比較例として、図１のフローチャートに示すような手法が考えられる。この比較例では、まず地磁気センサーや加速度センサー等から観測信号を受信し（Ｓ１）、次に観測信号を観測量に変換する（Ｓ２）。そして、観測量に表される磁北を基準とした方位角を、真北を基準とした方位角に偏角を用いて補正する（Ｓ３）。ここで、今回検出した観測信号に基づく方位角が誤っていると判断する場合には、上記の補正処理に加えて、今回検出した観測信号に基づく方位角と、期待する方位角との差などに基づき、観測量を補正する（Ｓ３）。そして、カルマンフィルターによる状態推定を行い（Ｓ４）、カルマンフィルターの内部状態変数の更新を行う（Ｓ５）。そして、一定時間周期で、Ｓ１〜Ｓ５の処理を繰り返す。つまり、本比較例は、今回検出した観測信号に基づく方位角が誤っていると判断された若しくは判断した時に、上記手段により、入力に含まれる誤差を軽減するという手法である。
【００３５】
しかし、比較例のように、観測量のみを補正した場合において、地磁気センサーによって検出される地磁気に同程度の誤差が混じり続ける場合には、図２（Ａ）〜図２（Ｄ）に示すように濾波した結果が変化する。図２（Ａ）は、ｋ回目の状態推定において、実際の真北の方向がＮ軸方向であるが、真北の方向をＥＶ１と濾波し、Ｎ軸とＥＶ１の差分となる角度がθ１である場合を示している。次に、図２（Ｂ）は、ｋ＋１回目の状態推定において、前回の誤差θ１を用いて、入力値を補正した後の濾波結果を示している。図２（Ｂ）では、真北の方向をＥＶ２と濾波し、Ｎ軸とＥＶ２との差分となる角度がθ２となっており、濾波結果が期待値に近づいている。そして、図２（Ｃ）は、ｋ＋２回目の状態推定結果を示している。この時も図２（Ｂ）と同様に、θ１で観測量を補正しているにも関わらず、濾波した真北方向ＥＶ３とＮ軸の差がθ３となり、θ２よりも大きくなってしまっている。さらに、図２（Ｄ）では、ｋ＋３回目の状態推定を行っているが、濾波した真北方向ＥＶ４とＮ軸の差であるθ４がθ１とほぼ同じになってしまっている。つまり、比較例のように、観測量のみを補正した場合には、元の誤った方位角に濾波結果が次第に戻っていってしまうという問題が発生する。
【００３６】
比較例において、図２のように濾波結果が変化してしまう原因は、カルマンフィルターの内部状態変数の中に、前回の状態推定結果を示す値などが含まれているにも関わらず、観測量のみを補正しており、観測量と内部状態変数の整合性が取れなくなっていることであると考えられる。
【００３７】
そこで、この比較例の問題点に対して、本出願人は、検出される観測信号に誤差が含まれると判断される場合において、観測量の他に内部状態変数も補正する手法を提案する。
【００３８】
本手法の概要を図３のフローチャートに示す。観測信号の受信（Ｓ１０）、信号変換（Ｓ１１）、観測量の補正（Ｓ１２）は、比較例と同様である。これらの処理を行った後に、今回検出した観測信号に基づく方位角と期待する方位角との差分などに基づき、内部状態変数の補正を行う（Ｓ１３）。そして、カルマンフィルターによる状態推定（Ｓ１４）、内部状態変数の更新（Ｓ１５）を行う。
【００３９】
以下、本実施形態のシステム構成例を説明し、その後、フローチャートを用いて本実施形態における処理の詳細について説明する。
【００４０】
２．カルマンフィルターの原理
以下、カルマンフィルターの概要を簡単に説明する。
【００４１】
カルマンフィルターは、直接観測不可能な値を、観測可能な値を用いて精度良く推定する際に用いられる状態推定器である。
【００４２】
例えば、ある時刻ｔｋにおいて、あるシステムの観測不可能な状態量Ｘｋを求めたい場合を考える。Ｘｋは直接観測できない値であるが、逆にＸｋを用いれば、観測可能な値である観測量Ｚｋを、（１）式で表せることが分かっている。
【００４３】
【数１】
【００４４】
（１）式において、ｋはステップ数、Ｘｋは時刻ｔｋにおけるシステムの状態推定値、Ｚｋは時刻ｔｋにおいて入力される観測量、Ｈｋは観測行列、Ｖｋは観測雑音を示している。なお、Ｈｋは物理法則や測定等によって予め確定した行列である。（１）式を観測方程式という。
【００４５】
また、時刻ｔｋにおけるシステムの状態量Ｘｋは、１ステップ前の時刻ｔｋ-１の状態量Ｘｋ-１をもとに、次のように表現されることが分かっているとする。
【００４６】
【数２】
【００４７】
（２）式において、Ｘｋ-１は時刻ｔｋ-１におけるシステムの状態量、Ｆｋは状態遷移行列、Ｂｋは駆動行列、Ｕｋは制御入力、Ｗｋはシステム雑音（白色雑音）を示している。なお、Ｆｋ及びＢｋは、物理法則や測定等によって予め確定した行列である。（２）式を状態方程式という。
【００４８】
ここで、（１）式に観測により得られた観測量Ｚｋを代入すれば、真の状態量Ｘｋが求められるようにも思えるが、観測雑音Ｖｋが不明であるため、（１）式から直接に真の状態量Ｘｋを求めることはできない。
【００４９】
そこで、まず、（２）式において、システム雑音Ｗｋがないと仮定した場合を考え、その場合の（２）式に相当する（３）式により、状態推定値Ｘｋ|ｋ-１を求める。
【００５０】
【数３】
【００５１】
（３）式において、Ｘｋ|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値であり、Ｘｋ-１|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋ-１のシステムの状態推定値である。
【００５２】
次に、（１）式において、観測誤差がないと仮定した場合を考え、その場合の（１）式に相当する（４）式により、観測誤差がないと仮定した場合の観測量の予測値Ｚｋ，eを求める。（４）式のＸｋ|ｋ-１は（３）式により求められたものである。
【００５３】
【数４】
【００５４】
そして、実際に観測により得られた観測量Ｚｋ，ｒとＺｋ，eとの差をとる。
【００５５】
【数５】
【００５６】
Δｋを観測残差という。観測残差は、以下の３値に起因する値である。一つ目の値は、雑音がないとした場合における、状態推定値から求めた観測量の推定値（観測推定量）と、実際の観測量との誤差である。そして、二つ目の値は、観測雑音Ｖｋであり、三つ目の値は、システム雑音Ｗｋである。
【００５７】
さらに、観測残差Δｋの共分散Ｓｋを算出する。観測残差Δｋの共分散Ｓｋは、システムの状態推定値Ｘｋから求められた観測推定量の確からしさと、観測雑音とシステム雑音がどの程度含まれているかを表すものである。つまり、観測残差Δｋの共分散Ｓｋをできるだけ小さくすることができれば、観測雑音とシステム雑音を抑圧しつつ、観測推定量を確からしいものにすることができる。さらに、観測推定量の正確性が担保されれば、観測推定量を求める際に参照された状態推定値の正確さも期待できる。
【００５８】
したがって、観測残差Δｋの共分散Ｓｋをできるだけ小さくするように制御すれば、システムの状態推定値も確からしいものとなる。
【００５９】
カルマンフィルターでは、以上の原理を基に、観測更新と予測処理との２つの処理をステップ毎に繰り返して、尤もらしい状態推定値Ｘｋを推定する。これらの処理の詳細は、以下のシステム構成において述べる。
【００６０】
３．システム構成例
次に、図４に本実施形態の状態推定装置及びこれを含む電子機器の構成例を示す。
【００６１】
状態推定装置１００は、信号変換部１２０と、制御部１４０と、補正部１６０と、状態推定部１８０と、記憶部２００と、を含む。また、状態推定装置１００を含む電子機器の例としては、センサー１０や、外部制御部１１、操作部１２、表示部１４などを含む携帯端末などが挙げられる。なお、状態推定装置１００及びこれを含む電子機器は、図４の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００６２】
ここで、センサー１０は、地磁気センサー等の方位センサーや、加速度センサー、ジャイロセンサー等のセンサーであり、観測信号を信号変換部１２０に出力する。方位センサーは、例えば地磁気センサー等であり、センサーの向いている方位を角度（０°〜３６０°）で計測する。地磁気センサーは、例えば磁場の強さによって抵抗値やインピーダンス値が増減する素子等で構成され、三軸の地磁気情報を検知する。加速度センサーは、例えば外力によって抵抗値が増減する素子等で構成され、三軸の加速度情報を検知する。ジャイロセンサーは、三軸の角速度情報を検知する。また、地磁気センサーや加速度センサー、ジャイロセンサーの機能を併せ持つセンサーを用いてもよい。さらに、地磁気情報や加速度情報から、磁北を基準とした偏角を算出し、出力するセンサーを用いてもよい。
【００６３】
ここで、地磁気とは、地球が持つ磁気及びそれにより地球上に生じる磁場をいう。地磁気はベクトル量であり、大きさと方向を持つ。地磁気（地磁気ベクトル）の大きさは全磁力によって表され、地磁気の方向は偏角と伏角により表される。偏角、伏角、全磁力をまとめて、地磁気の３要素という。図５のＮＥＤ座標系においては、ＤＲＧが地磁気ベクトルを示している。
【００６４】
また、一般的に、方位角を求める際には、「北」を基準にするが、地磁気センサーから直接求められる「北」は磁北であり、真北とは異なる。
【００６５】
磁北とは、地磁気ベクトルを水平面へ正射影した正射影ベクトルの方向のことである。言い換えると、地磁気のＳ極に対して、方位磁石の磁針のＮ極が反応して指し示す方位のことである。図５では、ＤＲＭＮの方向が磁北である。
【００６６】
一方、真北とは、地軸の北極点を指す方向のことを言う。一般的に、地理上で「北」と言えば、真北のことを指す。図５では、Ｎ軸方向が真北である。
【００６７】
真北と磁北はずれており、磁北と真北とが形成する角のことを偏角という。例えば、東京付近では磁北は真北に対して西に約７度ずれている。右手系のＮＥＤ座標系では、西に傾いている場合にマイナス、東に傾いている場合にプラスとなるように定義されるため、前述した例で言えば、東京付近では磁北は真北に対して−７度ずれているという。偏角は地域により異なる。図５では、θＤが偏角である。
【００６８】
さらに、地磁気ベクトルは、赤道付近を除けば、地面に対して平行ではなく、地面と斜めに交わっている。地磁気ベクトルを垂直面へ正射影した正射影ベクトルの方向と、水平面とが形成する角のことを伏角という。右手系のＮＥＤ座標系では、地磁気が水平面に向かって突き刺さる方向の場合がプラス、水平面から出て行く向きの場合がマイナスとなるように定義される。伏角も地域により異なる。図５では、θＩが伏角である。
【００６９】
そして、地磁気の大きさを全磁力という。偏角と伏角同様、全磁力も場所によって異なる。図５では、地磁気ベクトルＤＲＧの長さとして表されるＴＭＦが全磁力となる。
【００７０】
外部制御部１１は、例えば、本実施形態の状態推定装置を搭載した携帯電子機器等に設けられ、後述する補正条件の判断を行ってもよく、方位角補正データを制御部１４０に通知してもよい。操作部１２は、例えば本実施形態の状態推定装置を搭載した携帯電子機器等に設けられるボタン等であり、表示部１４は携帯電子機器の画面等である。
【００７１】
次に各部で行われる処理について説明する。
【００７２】
信号変換部１２０は、センサー１０から観測信号を取得し、観測量に変換する。そして、観測量を制御部１４０と補正部１６０とに出力する。
【００７３】
ここで、観測信号とは、センサーから直接得られる情報をいう。例えば、本実施形態では、観測信号Ｙを（６）式のように設定する。
【００７４】
【数６】
【００７５】
（６）式において、θ’は磁北を基準とした方位角である。また、「（ａｘ，ａｙ，ａｚ）」はボディー座標系で加速度センサーが検出する重力加速度である。「（ωｘ，ωｙ，ωｚ）」はボディー座標系でジャイロセンサーが検出する角速度である。ただし、観測信号はここで説明したものに限定されない。例えば、ボディー座標系で地磁気センサーが検出する地磁気である「（ｇｘ，ｇｙ，ｇｚ）」などを観測信号に含んでも良い。
【００７６】
また、観測量とは、カルマンフィルターの入力となる情報をいう。具体例については後述する。
【００７７】
制御部１４０は、補正部１６０と状態推定部１８０の制御を行う。
【００７８】
補正部１６０は、信号変換部１２０から取得した観測量のうち磁北を基準とした方位角θ’を、偏角θＤに基づき（７）式にて補正し、真北を基準とする方位角θを算出する。
【００７９】
【数７】
【００８０】
状態推定部１８０は、補正部１６０から取得した補正後の観測量を入力として、カルマンフィルターにより、方位角を推定する。
【００８１】
ここで、本実施形態では、観測量Ｚを次式（８）のように設定する。
【００８２】
【数８】
【００８３】
（８）式において、θは（７）式により求められた方位角である。「（ａｘ，ａｙ，ａｚ）」は（６）式における重力加速度と同一の値である。「（ＶＮ，ＶＥ，ＶＤ）」はＮＥＤ座標系における速度の南北方向（Ｎ）、東西方向（Ｅ）及び高度方向（Ｄ）の各成分である。「（ωｘ，ωｙ，ωｚ）」は（６）式における角速度と同一の値である。ただし、観測量はここで説明したものに限定されない。
【００８４】
また、状態量とは、システムの観測対象であるが、直接は観測不可能な値をいい、状態推定値とは、カルマンフィルターにより状態量を推定した値をいう。例えば、本実施形態では、カルマンフィルターにおいて推定対象とするシステムの状態推定値Ｘを次式（９）のように設定する。
【００８５】
【数９】
【００８６】
（９）式において、「（ｑ０，ｑ１，ｑ２，ｑ３）」はクォータニオン（四元数）を表し、「（ｖＮ，ｖＥ，ｖＤ）」はＮＥＤ座標系における速度の南北方向（Ｎ）、東西方向（Ｅ）及び高度方向（Ｄ）の各成分である。
【００８７】
なお、ここで説明したものに限定されず、状態量としてクォータニオンの代わりにロール・ピッチ・ヨー角を用いる方法や、速度の他に、状態量に緯度・経度・高度を追加する方法、状態量にジャイロや加速度のバイアス値を追加する方法などが可能である。
【００８８】
以上の前提条件の下に、カルマンフィルターでは、観測更新と予測処理との２つの処理を繰り返し行う。観測更新とは、外部から与えられる（実際に観測した）観測量Ｚｋ，ｒを利用して１ステップ前に予測した状態推定値Ｘｋ｜ｋ-１及び誤差共分散Ｐｋ｜ｋ-１を更新する処理である。一方、予測処理とは、更新した状態推定値Ｘｋ｜ｋ及び誤差共分散Ｐｋ｜ｋから次のステップの状態推定値Ｘｋ+１｜ｋ及び誤差共分散Ｐｋ+１｜ｋを予測する処理である。そして、カルマンフィルターは、この２つの処理を繰り返すことにより、尤もらしい状態推定値Ｘｋを推定する。
【００８９】
まず、観測更新は、次式（１０）〜（１３）で定式化される。
【００９０】
【数１０】
【００９１】
【数１１】
【００９２】
【数１２】
【００９３】
【数１３】
【００９４】
（１０）〜（１３）において、Ｓｋは、上述した観測残差Δｋの共分散である。Ｐｋ｜ｋ-１は、時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値Ｘｋ｜ｋ−１と、時刻ｔｋの実際の状態量Ｘｋとの差の共分散（誤差共分散）である。なお、Ｉは単位行列である。
【００９５】
また、観測行列Ｈｋは、状態推定値Ｘｋの次元を観測量Ｚｋの次元に変換するための変換行列として用いられる。観測行列Ｈｋを１ステップ前に予測された状態推定値Ｘｋ｜ｋ-１に作用させることで、観測量の予測値Ｚｋ，eが得られる。
【００９６】
本実施形態において、観測行列Ｈｋは以下のようになる。まず、θについては、（１４）式にて表すことができる。
【００９７】
【数１４】
【００９８】
次に、「（ａｘ，ａｙ，ａｚ）」については、（１５）式にて表すことができる。
【００９９】
【数１５】
【０１００】
そして、「（ｖＮ，ｖＥ，ｖＤ）」については、（１６）式にて表すことができる。
【０１０１】
【数１６】
【０１０２】
また、式（１１）において、Ｋｋはカルマンゲインと呼ばれる補正係数である。カルマンゲインＫｋは、予測された状態推定値Ｘｋ｜ｋ-１を重視するか、Ｚｋ，ｒとＺｋ，eとの差Δｋを重視するかを決定付けるパラメーター値として用いられる。カルマンゲインＫｋは、観測誤差Ｒｋの大きさによって決定付けられる。観測誤差Ｒｋは、観測量Ｚｋに含まれる想定誤差の大きさを表すパラメーター（想定誤差パラメーター）である。観測誤差Ｒｋは、期待値（平均値）を“０”とする正規分布型のホワイトノイズと仮定して設定される。
【０１０３】
式（１２）は、Ｚｋ，ｒとＺｋ，eとの差Δｋを用いて、状態推定値Ｘｋ｜ｋを更新することを表している。
【０１０４】
また、式（１３）は、１ステップ前に予測された誤差共分散Ｐｋ｜ｋ-１とカルマンゲインＫｋと観測行列Ｈｋとを用いて、誤差共分散Ｐｋ｜ｋを更新することを表している。
【０１０５】
一方、予測処理は、次式（１７）及び（１８）で定式化される。
【０１０６】
【数１７】
【０１０７】
【数１８】
【０１０８】
本実施例においては、ＢｋとＵｋは考慮しなくてもよい。また、（１８）式において、Ｑｋはシステム雑音と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。
【０１０９】
ここで、状態遷移行列Ｆｋは、観測更新で更新された状態推定値Ｘｋ|ｋから次のステップの状態推定値Ｘｋ+１|ｋへの遷移を表す行列として定義される。
【０１１０】
本実施形態における状態遷移行列Ｆｋは以下のようになる。まず、「（ｑ０，ｑ１，ｑ２，ｑ３）」については、（１９）式にて表すことができる。
【０１１１】
【数１９】
【０１１２】
次に、「（ｖＮ，ｖＥ，ｖＤ）」については、（２０）式にて表すことができる。
【０１１３】
【数２０】
【０１１４】
なお、（１９）式、（２０）式において、Δｔはサンプリング間隔である。例えば５０Ｈｚでサンプリングされる場合、Δｔ＝１／５０［ｓｅｃ］である。
【０１１５】
また、（３）式〜（５）式及び（１０）式〜（２０）式にて、表される値等をまとめて、カルマンフィルターの内部状態変数と呼ぶ。
【０１１６】
記憶部２００は、種々の情報を記憶するとともに、各部のワーク領域となるものであり、その機能はＲＡＭ等のメモリーやＨＤＤ（ハードディスクドライブ）などにより実現できる。
【０１１７】
以上の本実施形態では、観測により入力される観測信号に基づいて、観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部１８０と、観測信号を観測量に変換し、観測量を補正部１６０に出力する信号変換部１２０と、観測量の補正処理を行い、補正処理後の観測量を状態推定部１８０に出力する補正部１６０と、補正部１６０と状態推定部１８０を制御する制御部１４０と、を含む。そして、制御部１４０は、所定の補正条件が成立したと判断する場合、又は所定の補正条件が成立した旨を通知された場合に、補正部１６０及び状態推定部１８０に補正指示を行い、補正部１６０は、補正指示が行われた場合に、観測量の補正処理を行い、状態推定部１８０は、補正指示が行われた場合に、観測量に対応する内部状態変数の補正処理を行い、補正後の観測量と内部状態変数とに基づいて状態推定を行う。
【０１１８】
これにより、まず、入力された観測信号を観測量へ変換することができる。次に、補正条件が成立したと判断する場合、又は補正条件が成立した旨を他機能部や外部制御部１１から通知された場合に、観測量と内部状態変数の補正指示をし、これらの値を補正することができる。そして、補正した観測量を入力とし、内部状態変数を補正した状態で、カルマンフィルターにより、状態推定を行うことができる。
【０１１９】
ここで、補正条件とは、補正部１６０にて行う補正処理を決定付けるために設けられた判断基準である。詳しくは具体例とともに後述する。
【０１２０】
また、補正指示とは、補正条件が成立したことを通知するために、制御部１４０が補正部１６０及び状態推定部１８０に対して行う指示のことである。例えば、補正指示の一例として、補正条件成立時に、制御部１４０が補正部１６０及び状態推定部１８０に補正指示情報を出力することなどがある。なお、補正指示は、様々な変形が可能であり、これに限定されるものではない。例えば、補正条件成立時にのみ補正指示情報を出力するだけではなく、不成立時にも出力するが、補正指示情報に含まれる内容により、補正条件を満たしたか否かということが分かる方法でも良い。逆に、補正条件を満たさない時にのみ、制御部１４０が補正部１６０及び状態推定部１８０に補正指示情報を出力して、補正条件の成否を判断しても良い。他にも例えば、後述する方位角補正データの通知もって補正指示情報の通知としてもよい。
【０１２１】
以上より、センサー１０により検出された観測信号に基づく方位角（検出値）と期待する方位角（期待値）との誤差が大きい時に補正条件を満たすように設定をしておけば、検出値が誤っている場合等に、観測量だけでなく、内部状態変数のうち、前回推定した状態推定値も補正した状態で、新たに状態推定を行うことができるようになる。
【０１２２】
式（１２）に示すように、カルマンフィルターでは、前回推定した状態推定値を用いて、漸化式により現在の状態を濾波する。すなわち、前回推定した状態推定値の誤差が大きければ、現在の状態推定値の誤差も大きくなり、前回推定した状態推定値の誤差が小さければ、現在の状態推定値の誤差も小さくなる。例えば、上記で述べた比較例のように、観測量のみを補正した場合には、前回の状態推定値の誤差が補正されておらず、現在の状態推定値がその誤差の影響を受けてしまい、回を重ねる毎に誤差が蓄積され、元の誤った状態推定を行うようになってしまう。一方、本実施形態では、この前回の状態推定値の誤差を補正によって取り除き、前回の状態推定値を正しい値に書き換えているため、現在の状態推定において、前回までの状態推定の結果が反映されにくくなる。
【０１２３】
よって、観測量だけを補正して、状態推定を行う場合と比べて、補正処理後に状態推定を繰り返しても、状態推定値が補正前の値に戻りにくくすることができる。
【０１２４】
また、信号変換部１２０は、方位センサーからのセンサー信号を含む観測信号を、磁北を基準とする方位角の情報を含む観測量に変換してもよい。制御部１４０は、所定の補正条件が成立したと判断する場合、又は所定の補正条件が成立した旨を通知された場合に、補正部１６０及び状態推定部１８０に補正指示を行い、方位角補正データを通知してもよい。補正部１６０は、補正指示が行われた場合に、制御部１４０から通知された方位角補正データと、磁北を真北に補正する偏角とに基づいて、観測量の補正処理を行い、補正後の観測量を状態推定部１８０に出力してもよい。状態推定部１８０は、補正指示が行われた場合に、方位角補正データに基づいて、観測量に対応する内部状態変数の補正処理を行い、補正後の観測量と内部状態変数とに基づいて状態推定を行ってもよい。
【０１２５】
これにより、方位センサーからのセンサー信号を含む観測信号を、方位角の情報を含む観測量に変換することができる。そして、方位角の情報を含む観測量と内部状態変数を補正しながら、状態推定を行うことができるようになる。
【０１２６】
方位角補正データとは、センサー１０が検出した観測信号に基づく方位角（検出値）を、正しい方位角に補正するための情報である。例えば、検出値が磁北に対して−１０度（すなわち、右手系のＮＥＤ座標系で磁北に対して西に１０度傾いた方向）であった時に、外部制御部により正しい方位角が−７度であると判断された場合には、＋３度が方位角補正データとなる。なお、この例では角度で示したが、クォータニオン等により方位角補正データを表すことも当然可能である。また、真北を基準として、方位角補正データを算出してもよい。
【０１２７】
よって、本実施形態によれば、カルマンフィルターにより、方位角を推定しつつ、その推定した方位角が誤っている場合でも、補正処理により推定誤差をキャンセルし、ノイズを抑圧して、正しい方位角を推定することが可能となる。
【０１２８】
また、補正部１６０は、補正指示が行われた場合には、方位角補正データと偏角とに基づいて、観測量の補正処理を行い、補正指示が行われない場合には、偏角に基づいて、観測量の補正処理を行ってもよい。
【０１２９】
これにより、補正指示が行われた場合には、方位角補正データと偏角とに基づいて観測量を補正し、補正指示が行われない場合には、偏角によって観測量を補正して、状態推定を行うことができる。すなわち、本実施形態によれば、所定の補正条件が成立するか否かによって、観測量を補正する際に用いる値を変えることが可能となる。
【０１３０】
そのため、地磁気環境が悪い場合に、補正条件を満たすと判断する、又は判断されるようにしておけば、地磁気環境が悪い場合にのみ、方位角補正データと偏角とによって観測量を補正し、地磁気環境が良い場合には、通常用いる偏角のみを用いて観測量を補正することができるようになる。
【０１３１】
具体例を図６（Ａ）（Ｂ）に示す。図６（Ａ）（Ｂ）は、本実施形態が携帯端末ＤＶにて実現されており、ＤＶで方角推定を行う様子を示している。図６（Ａ）のように屋外で見晴らしが良く、周囲に地磁気に影響を及ぼすものがないような地磁気環境が良い場所においては、通常用いる偏角のみを用いて、観測量を補正する。これは、地磁気環境の悪い時に与えられた方位角補正データを補正処理に用いることにより、観測量に余計な修正を加える結果となってしまい、かえって状態推定値が期待値から離れてしまうことを避けるためである。
【０１３２】
一方、図６（Ｂ）に示すように、屋内等で周囲に電化製品等が沢山あるような地磁気環境の悪い場所では、方位角補正データと偏角を用いて、観測量を補正する。この場合には、通常用いる偏角のみで補正処理を行ったとしても、観測量に多く含まれる誤差を除去できないためである。
【０１３３】
したがって、補正条件の設定に応じて、適切な補正処理を行うことが可能となる。
【０１３４】
また、状態推定部１８０は、補正指示が行われた場合には、方位角補正データに基づいて、内部状態変数の補正処理を行い、補正指示が行われない場合には、内部状態変数の補正処理を行わなくてもよい。
【０１３５】
これにより、補正指示が行われない場合に、前回更新した値に補正を加えていない内部状態変数と、偏角により補正された観測量とを用いて、状態推定を行うこと等が可能となる。すなわち、この場合には、観測量も内部状態変数も方位角補正データによる補正を行っていないこととなる。よって、両変数の整合性を図ることができる。その結果、補正条件を満たさない場合、例えば図６（Ａ）のような地磁気環境が良い場合にも、ノイズを抑圧して、正しい方位角を推定することが可能となる。
【０１３６】
これまでに述べてきた方法により、所定の補正条件の判断により、異なる動作をする補正処理を行うことができる。以下では、補正条件の判断方法について述べる。
【０１３７】
地磁気センサーを用いて方位角を推定する場合、方位角の推定結果は地磁気の環境に大きな影響を受ける。すなわち、地磁気環境が良い場合と悪い場合に場合分けをして、異なる補正処理を行うことが好ましい。したがって、本実施形態では以下で述べる方法で地磁気環境の良し悪しを判断する。
【０１３８】
信号変換部１２０は、方位センサーから通知された観測信号に基づいて、全磁力を算出し、全磁力を制御部１４０に通知し、制御部１４０は、通知された全磁力に基づいて、所定の補正条件の判断をし、全磁力が所定の閾値により特定される範囲内にない場合には、所定の補正条件を満たすと判断し、全磁力が所定の閾値の範囲内にある場合には、所定の補正条件を満たさないと判断してもよい。
【０１３９】
これにより、方位センサーにより検出された観測信号に基づき、全磁力を算出することができ、その全磁力に基づいて、補正処理を行うか否かを決定すること等ができる。
【０１４０】
本来、地磁気が正しく検出された場合には、地磁気の強さを示す全磁力も全磁力の期待値に近い値になるはずである。すなわち、検出された全磁力（サンプル値）と全磁力の期待値とを比較し、サンプル値が期待値と十分に近いかどうかを判断することで、地磁気が正しく検出されたかどうかを判断することができる。さらに、地磁気が正しく検出された場合には、地磁気環境が良く、地磁気が正しく検出されなかった場合には地磁気環境が悪いと判断する。
【０１４１】
具体例を図７（Ａ）（Ｂ）に示す。図７（Ａ）（Ｂ）は、ＮＥＤ座標系において、実際の地磁気の方向（期待値）がＤＲＧである場合を示す。ここでは、観測信号に基づき、ＤＲＯ１及びＤＲＯ２、ＤＲＯ３に示す地磁気が導かれた場合を考える。図７（Ａ）に示すように、各地磁気の偏角はθＤで、同一であり、各地磁気の矢印の長さがそれぞれの全磁力を表している。
【０１４２】
図７（Ｂ）は、図７（Ａ）の地磁気の全磁力を縦軸とするグラフであり、ＤＲＧの全磁力をＴＭＦ、ＤＲＯ１〜ＤＲＯ３の全磁力をそれぞれＴＭＦ１〜ＴＭＦ３として示す。
【０１４３】
ここでは、全磁力のサンプル値と期待値が十分に近いかどうかを判断するために、任意の全磁力の閾値ＴｈｍａｘとＴｈｍｉｎを設定し、これらの閾値に挟まれる範囲ΔＦを設定する。サンプル値がΔＦ内に含まれれば、期待値ＴＭＦに近いと判断し、地磁気環境が良いと判断する。逆に、サンプル値がΔＦ内に含まれなければ、期待値ＴＭＦからかけ離れていると判断し、地磁気環境が悪いと判断する。図７（Ｂ）の例においては、ＴＭＦ２のみがΔＦ内に含まれているため、ＤＲＯ２の地磁気環境は良いと判断される。逆に、ＴＭＦ１とＴＭＦ３はどちらもΔＦ内に含まれないため、ＤＲＯ１とＤＯＲ３の地磁気環境は悪いと判断される。
【０１４４】
このように、本実施形態では、全磁力を用いることで、地磁気環境が良いか悪いかを判定して、補正処理を行うか否かを決定すること等ができる。
【０１４５】
また、信号変換部１２０は、方位センサーから通知された観測信号に基づいて、伏角を算出し、伏角を制御部１４０に通知し、制御部１４０は、通知された伏角に基づいて、所定の補正条件の判断をし、伏角が所定の閾値により特定される範囲内にない場合には、所定の補正条件を満たすと判断し、伏角が所定の閾値の範囲内にある場合には、所定の補正条件を満たさないと判断してもよい。
【０１４６】
これにより、方位センサーにより検出された観測信号に基づき、伏角を算出することができ、その伏角に基づいて、補正処理を行うか否かを決定すること等ができる。
【０１４７】
全磁力を使って、補正条件を判断する場合と同様に、本来、地磁気が正しく検出された場合には、伏角も伏角の期待値に近い値になるはずである。すなわち、検出された伏角のサンプル値と伏角の期待値とを比較し、サンプル値が期待値と十分に近いかどうかを判断することで、地磁気が正しく検出されたかどうかを判断することができる。
【０１４８】
具体例を図８（Ａ）（Ｂ）に示す。図８（Ａ）（Ｂ）は、ＮＥＤ座標系において、実際の地磁気の方向（期待値）がＤＲＧである場合を示す。ここでは、観測信号に基づき、ＤＲＯ４及びＤＲＯ５、ＤＲＯ６に示す地磁気が導かれた場合を考える。図８（Ａ）に示すように、各地磁気の偏角はθＤで、同一であり、Ｎ−Ｅ平面（Ｎ軸とＥ軸を含む平面）に対する各地磁気の矢印の角度がそれぞれの伏角を表している。
【０１４９】
図８（Ｂ）は、図８（Ａ）の地磁気の伏角を表した、Ｄ軸とＧ軸からなる二次元グラフであり、ＤＲＧの伏角をθＩ、ＤＲＯ４〜ＤＲＯ６の伏角をそれぞれθＯ４〜θＯ６として示す。
【０１５０】
ここでは、伏角のサンプル値と期待値が十分に近いかどうかを判断するために、ＤＲＧ方向を中心とする任意の角度範囲Δθを設定する。サンプル値がΔθ内に含まれれば、期待値θＩに近いと判断し、地磁気環境が良いと判断する。逆に、サンプル値がΔθ内に含まれなければ、期待値θＩからかけ離れていると判断し、地磁気環境が悪いと判断する。図８（Ｂ）の例においては、θＯ５のみがΔθ内に含まれているため、ＤＲＯ５の地磁気環境は良いと判断される。逆に、θＯ４とθＯ６はどちらもΔθ内に含まれないため、θＯ４とθＯ６の地磁気環境は悪いと判断される。
【０１５１】
このように、本実施形態では、伏角を用いることで、地磁気環境が良いか悪いかを判定して、補正処理を行うか否かを決定すること等ができる。
【０１５２】
また、制御部１４０は、信号変換部１２０から取得した観測量をマップマッチング部１６に出力し、マップマッチング部１６からマップマッチングの結果を取得し、マップマッチングの結果に基づいて、所定の補正条件を判断してもよい。
【０１５３】
これにより、マップマッチングの結果に基づいて、補正処理を行うか否かを決定すること等ができる。
【０１５４】
ここで、マップマッチングの結果とは、センサー１０が検出した観測信号に基づいて算出された方位角（検出値）と、方位角の期待値とを、外部に設けられたマップマッチング部１６が比較した結果である。マップマッチングの結果には、検出値と期待値が一致しているか否かといった情報や、検出値と期待値の差等が含まれる。
【０１５５】
具体例を図９（Ａ）（Ｂ）に示す。図９（Ａ）は、マップマッチングの結果を使って補正条件を判定する実施形態のシステム構成の一例である。図４に示すブロック図に加えて、本実施形態の状態推定装置１００の外部にマップマッチング部１６があり、制御部１４０はマップマッチング部１６から通知されるマップマッチングの結果を取得して、補正条件の判定を行う。
【０１５６】
図９（Ｂ）には、ユーザーＵＳが持つ端末が本実施形態を実現しており、図に示す位置において、方位角を検出し、マップマッチングを行う様子を示している。この場合に、例えば、ＤＲＥ方向が真北であるとユーザーＵＳの端末が検出したとする。すると、マップマッチング部１６は、実際の真北の方角ＤＲＮと、検出した方角ＤＲＥとを比較し、一致しているかどうかを判断し、その結果を制御部１４０に通知する。制御部１４０は、マップマッチングの結果、方角が一致していると判定された場合には、補正指示を行わないと判断し、方角が一致していないと判定された場合には、補正指示を行うと判断し、補正部１６０と状態推定部１８０に補正指示を通知する。
【０１５７】
以上のように、本実施形態では、マップマッチングの結果を用い、現在検出した方位角が正しいか否かを判定することにより、地磁気環境が良いか悪いかを判定して、補正処理を行うか否かを決定すること等ができる。
【０１５８】
また、上記で述べた補正条件が成立しない場合でも、推定した方位角が間違っていると感じるユーザーもいる。
【０１５９】
そこで、制御部１４０は、操作部１２から入力されたユーザー入力情報に基づいて、方位角補正データを特定し、補正部１６０及び状態推定部１８０に補正指示と方位角補正データとを通知し、補正部１６０は、補正指示が行われた場合には、方位角補正データと、偏角とに基づいて、観測量の補正処理を行い、状態推定部１８０は、補正指示が行われた場合には、方位角補正データに基づいて、内部状態変数の補正処理を行ってもよい。
【０１６０】
これにより、ユーザー入力情報に基づいて、方位角補正データを特定することができる。そして、ユーザーからの入力があった場合には、所定の補正条件の成立の如何に関わらず、補正部１６０と状態推定部１８０に、補正指示を行うことができ、この際に特定した方位角補正データも通知することができる。
【０１６１】
ここで、ユーザー入力情報とは、ユーザーが状態推定部１８０の出力として期待する方位角と、実際に状態推定部１８０が推定した方位角との差の情報を含む情報であり、操作部１２を通じてユーザーにより入力される情報である。
【０１６２】
以上より、推定した方位角が間違っているとユーザーが感じた場合にも、ユーザーが補正データを入力すること等によって、本実施形態の状態推定装置に補正指示を出すことを促すことができる。その結果、ユーザーのニーズに応じた正確性を有する方位角の推定を行うことができる。
【０１６３】
なお本実施形態の状態推定装置等は、プログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態の状態推定装置等が実現される。具体的には情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そしてＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。
【０１６４】
４．処理の流れ
以下では、図１０及び図１１のフローチャートを用いて、本実施形態の処理の流れについて説明する。
【０１６５】
４．１． マップマッチング結果に基づき補正条件を判断する手法の詳細
まず、信号変換部１２０が、センサー１０の地磁気センサー及び加速度センサー、ジャイロセンサー等からそれぞれ三軸の観測信号を取得する（Ｓ２０）。次に、信号変換部１２０は、観測信号に基づいて、磁北を基準とした方位角の情報を含む観測量を算出し（Ｓ２１）、制御部１４０と補正部１６０に観測量を出力する。そして、制御部１４０は、観測量に表される方位角をマップマッチング部１６に出力する（Ｓ２２）。その後、制御部１４０は、マップマッチング部１６から補正指示があるか否かを判断する（Ｓ２３）。制御部１４０は、マップマッチング部１６から補正指示を受けたと判断する場合には、補正部１６０と状態推定部１８０に補正指示をし、さらに、マップマッチング部１６から取得した方位角補正データを通知する。そして、補正部１６０は、方位角補正データと偏角とに基づき、観測量を補正し、状態推定部１８０は、方位角補正データに基づき内部状態変数を補正する（Ｓ２４）。一方、制御部１４０は、マップマッチング部１６から補正指示を受けていないと判断する場合には、補正指示を行わず、補正部１６０は、偏角に基づき、観測量を補正する（Ｓ２５）。観測量の補正処理が終わると、補正処理後の観測量を入力として、カルマンフィルターにより状態推定を行う（Ｓ２６）。そして、最後に、カルマンフィルターの内部状態変数の更新を行う（Ｓ２７）。
【０１６６】
本実施形態では、以上の処理を一定時間周期で行う。なお、マップマッチング部に方位角を通知し、マップマッチングを行う処理は毎回行う必要はない。この場合においては、マップマッチングを行わない場合には、前回マップマッチングを行った時と同様の補正指示と方位角補正データに基づいて補正処理を行ってもよい。
【０１６７】
４．２． 全磁力に基づき補正条件を判断する手法の詳細
まず、信号変換部１２０が、センサー１０の地磁気センサー及び加速度センサー、ジャイロセンサー等からそれぞれ三軸の観測信号を取得する（Ｓ３０）。次に、信号変換部１２０は、観測信号に基づいて、磁北を基準とした方位角と全磁力との情報を含む観測量を算出し（Ｓ３１）、制御部１４０と補正部１６０に観測量を出力する。そして、制御部１４０は、通知された全磁力が所定の範囲内である否かを判断する（Ｓ３２）。制御部１４０は、全磁力が所定の範囲内にないと判断した場合には、その旨を外部制御部に通知する（Ｓ３３）。制御部１４０が、全磁力が所定の範囲内にあると判断した場合、又はＳ３３の処理後において、制御部１４０は、外部制御部からの補正指示があるか否かを判断する（Ｓ３４）。制御部１４０は、外部制御部１１から補正指示を受けたと判断する場合には、補正部１６０と状態推定部１８０に補正指示をし、さらに、外部制御部１１から取得した方位角補正データを通知する。そして、補正部１６０は、方位角補正データと偏角とに基づき、観測量を補正し、状態推定部１８０は、方位角補正データに基づき内部状態変数を補正する（Ｓ３５）。一方、制御部１４０は、外部制御部１１から補正指示を受けていないと判断する場合には、補正指示を行わず、補正部１６０は、偏角に基づき、観測量を補正する（Ｓ３６）。観測量の補正処理が終わると、補正処理後の観測量を入力として、カルマンフィルターにより状態推定を行う（Ｓ３７）。そして、最後に、カルマンフィルターの内部状態変数の更新を行う（Ｓ３８）。
【０１６８】
本実施形態では、以上の処理を一定時間周期で行う。なお、制御部１４０による、全磁力が所定の範囲内にあるか否かを判断する処理は毎回行う必要はない。この場合においては、上記処理を行わない場合には、前回上記処理を行った時と同様の補正指示と方位角補正データに基づいて補正処理を行ってもよい。
【０１６９】
以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また状態推定装置及びこれを含む電子機器の構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１７０】
１０ センサー、１１ 外部制御部、１２ 操作部、１４ 表示部、
１６ マップマッチング部、１００ 状態推定装置、１２０ 信号変換部、
１４０ 制御部、１６０ 補正部、１８０ 状態推定部、２００ 記憶部
【特許請求の範囲】
【請求項１】
観測により入力される観測信号に基づいて、前記観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部と、
前記観測信号を観測量に変換し、前記観測量を補正部に出力する信号変換部と、
前記観測量の補正処理を行い、補正処理後の前記観測量を前記状態推定部に出力する補正部と、
前記補正部と前記状態推定部を制御する制御部と、
を含み、
前記制御部は、
所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、
前記補正部は、
前記補正指示が行われた場合に、前記観測量の補正処理を行い、
前記状態推定部は、
前記補正指示が行われた場合に、前記観測量に対応する内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とする状態推定装置。
【請求項２】
請求項１において、
前記信号変換部は、
方位センサーからのセンサー信号を含む観測信号を、磁北を基準とする方位角の情報を含む観測量に変換し、
前記制御部は、
前記所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、方位角補正データを通知し、
前記補正部は、
前記補正指示が行われた場合に、前記制御部から通知された前記方位角補正データと、磁北を真北に補正する偏角とに基づいて、前記観測量の補正処理を行い、補正後の前記観測量を前記状態推定部に出力し、
前記状態推定部は、
前記補正指示が行われた場合に、前記方位角補正データに基づいて、前記観測量に対応する前記内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とする状態推定装置。
【請求項３】
請求項２において、
前記補正部は、
前記補正指示が行われた場合には、
前記方位角補正データと前記偏角とに基づいて、前記観測量の補正処理を行い、
前記補正指示が行われない場合には、
前記偏角に基づいて、前記観測量の補正処理を行うことを特徴とする状態推定装置。
【請求項４】
請求項３において、
前記状態推定部は、
前記補正指示が行われた場合には、
前記方位角補正データに基づいて、前記内部状態変数の補正処理を行い、
前記補正指示が行われない場合には、
前記内部状態変数の補正処理を行わないことを特徴とする状態推定装置。
【請求項５】
請求項２乃至４のいずれかにおいて、
前記信号変換部は、
前記方位センサーから通知された観測信号に基づいて、全磁力を算出し、前記全磁力を前記制御部に通知し、
前記制御部は、
通知された前記全磁力に基づいて、前記所定の補正条件の判断をし、前記全磁力が所定の閾値により特定される範囲内にない場合には、前記所定の補正条件を満たすと判断し、前記全磁力が前記所定の閾値の範囲内にある場合には、前記所定の補正条件を満たさないと判断することを特徴とする状態推定装置。
【請求項６】
請求項２乃至４のいずれかにおいて、
前記信号変換部は、
前記方位センサーから通知された観測信号に基づいて、伏角を算出し、前記伏角を前記制御部に通知し、
前記制御部は、
通知された前記伏角に基づいて、前記所定の補正条件の判断をし、前記伏角が所定の閾値により特定される範囲内にない場合には、前記所定の補正条件を満たすと判断し、前記伏角が前記所定の閾値の範囲内にある場合には、前記所定の補正条件を満たさないと判断することを特徴とする状態推定装置。
【請求項７】
請求項２乃至４のいずれかにおいて、
前記制御部は、
前記信号変換部から取得した前記観測量をマップマッチング部に出力し、マップマッチング部からマップマッチングの結果を取得し、前記マップマッチングの結果に基づいて、前記所定の補正条件を判断することを特徴とする状態推定装置。
【請求項８】
請求項２乃至７のいずれかにおいて、
前記制御部は、
操作部から入力されたユーザー入力情報に基づいて、前記方位角補正データを特定し、前記補正部及び前記状態推定部に前記補正指示と前記方位角補正データとを通知し、
前記補正部は、
前記補正指示が行われた場合には、前記方位角補正データと、前記偏角とに基づいて、前記観測量の補正処理を行い、
前記状態推定部は、
前記補正指示が行われた場合には、前記方位角補正データに基づいて、前記内部状態変数の補正処理を行うことを特徴とする状態推定装置。
【請求項９】
請求項１乃至８のいずれかに記載の状態推定装置を含むことを特徴とする電子機器。
【請求項１０】
観測により入力される観測信号に基づいて、前記観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部と、
前記観測信号を観測量に変換し、前記観測量を補正部に出力する信号変換部と、
前記観測量の補正処理を行い、補正処理後の前記観測量を前記状態推定部に出力する補正部と、
前記補正部と前記状態推定部を制御する制御部として、
コンピューターを機能させ、
前記制御部は、
所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、
前記補正部は、
前記補正指示が行われた場合に、前記観測量の補正処理を行い、
前記状態推定部は、
前記補正指示が行われた場合に、前記観測量に対応する内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とするプログラム。
【請求項１】
観測により入力される観測信号に基づいて、前記観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部と、
前記観測信号を観測量に変換し、前記観測量を補正部に出力する信号変換部と、
前記観測量の補正処理を行い、補正処理後の前記観測量を前記状態推定部に出力する補正部と、
前記補正部と前記状態推定部を制御する制御部と、
を含み、
前記制御部は、
所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、
前記補正部は、
前記補正指示が行われた場合に、前記観測量の補正処理を行い、
前記状態推定部は、
前記補正指示が行われた場合に、前記観測量に対応する内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とする状態推定装置。
【請求項２】
請求項１において、
前記信号変換部は、
方位センサーからのセンサー信号を含む観測信号を、磁北を基準とする方位角の情報を含む観測量に変換し、
前記制御部は、
前記所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、方位角補正データを通知し、
前記補正部は、
前記補正指示が行われた場合に、前記制御部から通知された前記方位角補正データと、磁北を真北に補正する偏角とに基づいて、前記観測量の補正処理を行い、補正後の前記観測量を前記状態推定部に出力し、
前記状態推定部は、
前記補正指示が行われた場合に、前記方位角補正データに基づいて、前記観測量に対応する前記内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とする状態推定装置。
【請求項３】
請求項２において、
前記補正部は、
前記補正指示が行われた場合には、
前記方位角補正データと前記偏角とに基づいて、前記観測量の補正処理を行い、
前記補正指示が行われない場合には、
前記偏角に基づいて、前記観測量の補正処理を行うことを特徴とする状態推定装置。
【請求項４】
請求項３において、
前記状態推定部は、
前記補正指示が行われた場合には、
前記方位角補正データに基づいて、前記内部状態変数の補正処理を行い、
前記補正指示が行われない場合には、
前記内部状態変数の補正処理を行わないことを特徴とする状態推定装置。
【請求項５】
請求項２乃至４のいずれかにおいて、
前記信号変換部は、
前記方位センサーから通知された観測信号に基づいて、全磁力を算出し、前記全磁力を前記制御部に通知し、
前記制御部は、
通知された前記全磁力に基づいて、前記所定の補正条件の判断をし、前記全磁力が所定の閾値により特定される範囲内にない場合には、前記所定の補正条件を満たすと判断し、前記全磁力が前記所定の閾値の範囲内にある場合には、前記所定の補正条件を満たさないと判断することを特徴とする状態推定装置。
【請求項６】
請求項２乃至４のいずれかにおいて、
前記信号変換部は、
前記方位センサーから通知された観測信号に基づいて、伏角を算出し、前記伏角を前記制御部に通知し、
前記制御部は、
通知された前記伏角に基づいて、前記所定の補正条件の判断をし、前記伏角が所定の閾値により特定される範囲内にない場合には、前記所定の補正条件を満たすと判断し、前記伏角が前記所定の閾値の範囲内にある場合には、前記所定の補正条件を満たさないと判断することを特徴とする状態推定装置。
【請求項７】
請求項２乃至４のいずれかにおいて、
前記制御部は、
前記信号変換部から取得した前記観測量をマップマッチング部に出力し、マップマッチング部からマップマッチングの結果を取得し、前記マップマッチングの結果に基づいて、前記所定の補正条件を判断することを特徴とする状態推定装置。
【請求項８】
請求項２乃至７のいずれかにおいて、
前記制御部は、
操作部から入力されたユーザー入力情報に基づいて、前記方位角補正データを特定し、前記補正部及び前記状態推定部に前記補正指示と前記方位角補正データとを通知し、
前記補正部は、
前記補正指示が行われた場合には、前記方位角補正データと、前記偏角とに基づいて、前記観測量の補正処理を行い、
前記状態推定部は、
前記補正指示が行われた場合には、前記方位角補正データに基づいて、前記内部状態変数の補正処理を行うことを特徴とする状態推定装置。
【請求項９】
請求項１乃至８のいずれかに記載の状態推定装置を含むことを特徴とする電子機器。
【請求項１０】
観測により入力される観測信号に基づいて、前記観測を行ったシステムの内部状態をカルマンフィルターにより推定する状態推定部と、
前記観測信号を観測量に変換し、前記観測量を補正部に出力する信号変換部と、
前記観測量の補正処理を行い、補正処理後の前記観測量を前記状態推定部に出力する補正部と、
前記補正部と前記状態推定部を制御する制御部として、
コンピューターを機能させ、
前記制御部は、
所定の補正条件が成立したと判断する場合、又は前記所定の補正条件が成立した旨を通知された場合に、前記補正部及び前記状態推定部に補正指示を行い、
前記補正部は、
前記補正指示が行われた場合に、前記観測量の補正処理を行い、
前記状態推定部は、
前記補正指示が行われた場合に、前記観測量に対応する内部状態変数の補正処理を行い、前記補正後の観測量と前記内部状態変数とに基づいて状態推定を行うことを特徴とするプログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−１９８１１４（Ｐ２０１２−１９８１１４Ａ）
【公開日】平成２４年１０月１８日（２０１２．１０．１８）
【国際特許分類】
【出願番号】特願２０１１−６２６９６（Ｐ２０１１−６２６９６）
【出願日】平成２３年３月２２日（２０１１．３．２２）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年１０月１８日（２０１２．１０．１８）
【国際特許分類】
【出願日】平成２３年３月２２日（２０１１．３．２２）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]