JP2012203566A - 状態推定装置、電子機器及びプログラム - Google Patents

Abstract

【課題】 カルマンフィルターの演算において、固定小数点数を用い、固定小数点位置を調整しながら演算を行うことにより、計算量を抑えつつ、演算の破綻を回避できる状態推定装置等の提供。
【解決手段】 状態推定装置１００は、カルマンフィルターの演算処理を行う演算部１２０と、演算部１２０を制御する制御部１４０と、演算結果を記憶する記憶部１６０と、を含む。そして、演算部１２０は、演算処理の中で、固定小数点数として表現される誤差共分散の演算処理を行う。制御部１４０は、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。
【選択図】 図１

Description

本発明は、状態推定装置、電子機器及びプログラム等に関係する。

カルマンフィルターにおいて、内部状態変数の演算処理を行う方法として、内部状態変数や観測量を固定小数点数により表現して演算を行う手法と、内部状態変数や観測量を浮動小数点数により表現して演算を行う手法が考えられる。

固定小数点数により表現して演算を行う手法では、小数点位置を固定的に決めておき、表現可能な範囲の値を用いて演算を行う。例えば、３２ビットにより表現される固定小数点数において、２の補数表現で符号ビットを考慮し、固定小数点位置がビット番号３０と３１の間であるとする場合には、［−１，＋１）の範囲の値が表現できる。この時、最小分解能は、１／２^３１である。

また、適応フィルターの演算処理において、固定小数点数を用いる発明としては特許文献１に記載される従来技術がある。

特開２００９―３０２８８５号公報

カルマンフィルターにおいて、内部状態変数や観測量を固定小数点数により表現して演算を行う手法には、演算中にオーバーフローが発生し、演算結果が不定値となり、演算が破綻する場合があるという問題点がある。

一方、内部状態変数や観測量を浮動小数点数により表現して演算を行う手法では、演算を破綻させずに行うことはできるものの、計算量が増大してしまうという問題点がある。

本発明の幾つかの態様によれば、カルマンフィルターの演算において、固定小数点数を用い、固定小数点位置を調整しながら演算を行うことにより、計算量を抑えつつ、演算の破綻を回避できる状態推定装置、電子機器及びプログラム等を提供できる。

本発明の一態様は、カルマンフィルターの演算処理を行う演算部と、前記演算部を制御する制御部と、カルマンフィルターの演算処理の演算結果を記憶する記憶部と、を含み、前記演算部は、前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、前記制御部は、過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とする状態推定装置に関係する。

本発明の一態様では、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。よって、誤差共分散の固定小数点位置を調整しつつ、カルマンフィルターの演算処理を行うことが可能となる。

本発明の一態様では、前記制御部は、前記誤差共分散についての過去の前記演算結果に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。

これにより、過去の誤差共分散の演算結果に基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。

また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における上位ビットに基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。

これにより、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。

また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第一の閾値以上である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、前記右シフト量を前記演算部に通知し、前記演算部は、前記制御部から前記右シフト量を通知された場合に、前記右シフト量に基づき、前記誤差共分散を算術右シフトしてもよい。

これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第一の閾値を超えている場合には、誤差共分散を算術右シフトし、カルマンフィルターの演算処理中にオーバーフローが発生しないように固定小数点位置を調整すること等が可能になる。

また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第二の閾値以下である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、前記左シフト量を前記演算部に通知し、前記演算部は、前記制御部から前記左シフト量を通知された場合に、前記左シフト量に基づき、前記誤差共分散を算術左シフトしてもよい。

これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第二の閾値以下である場合には、誤差共分散を算術左シフトし、カルマンフィルターの演算処理の正確度を高めるように固定小数点位置を調整すること等が可能になる。

また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の正負判断を行い、過去の前記誤差共分散の正負に応じて、過去の前記誤差共分散に任意のビット列を加算し、前記ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。

これにより、誤差共分散の絶対値を求めずに、誤差共分散の正負の判断を行い、任意のビット列を加算して、その際のオーバーフローフラグの値に基づいて、誤差共分散の固定小数点位置を調整すること等が可能になる。

また、本発明の一態様では、前記演算部は、飽和演算命令により、前記誤差共分散の前記演算処理を行い、前記制御部は、前記演算処理において、飽和が発生したと判断する場合には、前記誤差共分散の前記固定小数点位置を変更するためのシフト量を決定し、前記シフト量を演算部に通知し、前記演算部は、前記制御部から通知された前記シフト量に基づいて、前記演算処理前の前記誤差共分散を算術シフトし、前記固定小数点位置を変更してもよい。

これにより、カルマンフィルターの演算処理を飽和演算により行い、演算処理中に飽和が発生したか否かによって、固定小数点位置を変更する否かを決定し、固定小数点位置を調整すること等が可能になる。

また、本発明の一態様では、前記演算部は、前記誤差共分散が行列で表される場合に、前記演算処理の中で、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、前記制御部は、前記誤差共分散が行列で表される場合に、過去の前記誤差共分散行列の各要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うってもよい。

これにより、誤差共分散が行列で表される場合に、誤差共分散行列の各要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。

また、本発明の一態様では、前記制御部は、前記誤差共分散が行列で表される場合に、過去の前記誤差共分散行列の対角要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行ってもよい。

これにより、誤差共分散が行列で表される場合に、誤差共分散行列の対角要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。

また、本発明の一態様では、前記演算部は、前記誤差共分散及び状態推定値の予測処理と観測更新を行い、前記制御部は、前記予測処理における前記誤差共分散の前記固定小数点位置の前記決定処理を行い、前記観測更新における前記誤差共分散の前記固定小数点位置の前記決定処理を行ってもよい。

これにより、観測更新と予測処理のそれぞれについて、誤差共分散の固定小数点位置を調整すること等が可能になる。

また、本発明の一態様では、前記制御部は、Ｐ＝ＦＰＦ^Ｔ＋Ｑの演算処理により、前記誤差共分散Ｐを求める場合に、ＦＰＦ^Ｔ及びＱの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行ってもよい。

これにより、Ｐ＝ＦＰＦ^Ｔ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦ^Ｔ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＦＰＦ^Ｔ及びＱの固定小数点位置の調整すること等が可能になる。

また、本発明の一態様では、前記制御部は、Ｓ＝ＨＰＨ^Ｔ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨ^Ｔ及びＲの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行ってもよい。

これにより、Ｓ＝ＨＰＨ^Ｔ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨ^Ｔ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＨＰＨ^Ｔ及びＲの固定小数点位置の調整すること等が可能になる。

また、本発明の他の態様では、前記状態推定装置を含むことを特徴とする電子機器に関係する。

また、本発明の他の態様では、上記各部としてコンピューターを機能させるプログラム、又は該プログラムを記憶したコンピューター読み取り可能な情報記憶媒体に関係する。

本実施形態の状態推定装置の構成例。 固定小数点と上位ビットの説明図。 図３（Ａ）、図３（Ｂ）は、算術右シフトにより固定小数点位置を変更する手法の説明図。 図４（Ａ）、図４（Ｂ）は、算術左シフトにより固定小数点位置を変更する手法の説明図。 図５（Ａ）〜図５（Ｄ）は、Ｐが正の値である場合に、第１のビット列を用いて固定小数点位置を変更する手法の説明図。 図６（Ａ）〜図６（Ｄ）は、Ｐが正の値である場合に、第２のビット列を用いて固定小数点位置を変更する手法の説明図。 図７（Ａ）〜図７（Ｄ）は、Ｐが負の値である場合に、第３のビット列を用いて固定小数点位置を変更する手法の説明図。 図８（Ａ）〜図８（Ｄ）は、Ｐが負の値である場合に、第４のビット列を用いて固定小数点位置を変更する手法の説明図。 固定小数点位置を調整しながらカルマンフィルターの演算を行う手法のフローチャート。 固定小数点位置を調整する手法のフローチャート。

以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。

１．概要
本実施形態では、カルマンフィルターの演算において、観測量や内部状態変数などを固定小数点数として扱い、演算を行う。これにより、内部状態変数等を浮動小数点数として演算を行う場合と比較して、計算量の削減を図ることができる。

また、本実施形態では、カルマンフィルターの演算において、オーバーフローが発生しないように、誤差共分散の固定小数点位置を動的に変更する。誤差共分散は、カルマンフィルターの演算の中で、最もダイナミックレンジが広いと考えられるためである。これにより、誤差共分散の演算中にオーバーフローが発生し、演算結果が不定値となり、演算が破綻することを防ぐことができる。

さらに、本実施形態では、誤差共分散以外の内部状態変数の一部又は全てについて、固定小数点位置を誤差共分散の固定小数点位置と一致させる。これにより、状態推定値や観測残差等の内部状態変数の固定小数点位置を、計算量を増加させずに、オーバーフローが発生しないように調整することができる。

以下、本実施形態のシステム構成例を説明し、その後、フローチャートを用いて本実施形態の処理の詳細について説明する。

２．システム構成例
次に、図１に本実施形態の状態推定装置及びこれを含む電子機器の構成例を示す。

状態推定装置１００は、演算部１２０と、制御部１４０と、記憶部１６０と、を含む。また、状態推定装置１００を含む電子機器の例としては、センサーや外部制御部や操作部や表示部などを含む携帯端末などが挙げられる。なお、状態推定装置１００及びこれを含む電子機器は、図１の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。

次に各部で行われる処理について説明する。

演算部１２０は、カルマンフィルターの演算処理を行う。制御部１４０は、演算部１２０を制御する。記憶部１６０は、カルマンフィルターの演算処理の演算結果を記憶する。

ここで、カルマンフィルターの概要を簡単に説明する。

カルマンフィルターは、直接観測不可能な値を、観測可能な値を用いて精度良く推定する際に用いられる状態推定器である。

例えば、ある時刻ｔ_ｋにおいて、あるシステムの観測不可能な状態量Ｘ_ｋを求めたい場合を考える。Ｘ_ｋは直接観測できない値であるが、逆にＸ_ｋを用いれば、観測可能な値である観測量Ｚ_ｋを、（１）式で表せることが分かっている。

（１）式において、ｋはステップ数、Ｘ_ｋは時刻ｔ_ｋにおけるシステムの状態推定値、Ｚ_ｋは時刻ｔ_ｋにおいて入力される観測量、Ｈ_ｋは観測行列、Ｖ_ｋは観測雑音を示している。なお、Ｈ_ｋは物理法則や測定等によって予め確定した行列である。（１）式を観測方程式という。

また、時刻ｔ_ｋにおけるシステムの状態量Ｘ_ｋは、１ステップ前の時刻ｔ_ｋ-１の状態量Ｘ_ｋ-１をもとに、次のように表現されることが分かっているとする。

（２）式において、Ｘ_ｋ-１は時刻ｔ_ｋ-１におけるシステムの状態量、Ｆ_ｋは状態遷移行列、Ｂ_ｋは駆動行列、Ｕ_ｋは制御入力、Ｗ_ｋはシステム雑音（白色雑音）を示している。なお、Ｆ_ｋ及びＢ_ｋは、物理法則や測定等によって予め確定した行列である。（２）式を状態方程式という。

ここで、（１）式に観測により得られた観測量Ｚ_ｋを代入すれば、真の状態量Ｘ_ｋが求められるようにも思えるが、観測雑音Ｖ_ｋが不明であるため、（１）式から直接に真の状態量Ｘ_ｋを求めることはできない。

そこで、まず、（２）式において、システム雑音Ｗ_ｋがないと仮定した場合を考え、その場合の（２）式に相当する（３）式により、状態推定値Ｘ_ｋ|ｋ-１を求める。

（３）式において、Ｘ_ｋ|ｋ-１は時刻ｔ_ｋ-１において推定した時刻ｔ_ｋのシステムの状態推定値であり、Ｘ_{ｋ-１|ｋ-１}は時刻ｔ_ｋ-１において推定した時刻ｔ_ｋ-１のシステムの状態推定値である。

次に、（１）式において、観測誤差がないと仮定した場合を考え、その場合の（１）式に相当する（４）式により、観測誤差がないと仮定した場合の観測量の予測値Ｚ_ｋ，eを求める。（４）式のＸ_ｋ|ｋ-１は（３）式により求められたものである。

そして、実際に観測により得られた観測量Ｚ_ｋ，ｒとＺ_ｋ，eとの差をとる。

Δ_ｋを観測残差という。観測残差は、以下の３値に起因する値である。一つ目の値は、雑音がないとした場合における、状態推定値から求めた観測量の推定値（観測推定量）と、実際の観測量との誤差である。そして、二つ目の値は、観測雑音Ｖ_ｋであり、三つ目の値は、システム雑音Ｗ_ｋである。

さらに、観測残差Δ_ｋの共分散Ｓ_ｋを算出する。観測残差Δ_ｋの共分散Ｓ_ｋは、システムの状態推定値Ｘ_ｋから求められた観測推定量の確からしさと、観測雑音とシステム雑音がどの程度含まれているかを表すものである。つまり、観測残差Δ_ｋの共分散Ｓ_ｋをできるだけ小さくすることができれば、観測雑音とシステム雑音を抑圧しつつ、観測推定量を確からしいものにすることができる。さらに、観測推定量の正確性が担保されれば、観測推定量を求める際に参照された状態推定値の正確さも期待できる。

したがって、観測残差Δ_ｋの共分散Ｓ_ｋをできるだけ小さくするように制御すれば、システムの状態推定値も確からしいものとなる。

カルマンフィルターでは、以上の原理を基に、観測更新と予測処理との２つの処理を繰り返し行う。観測更新とは、外部から与えられる（実際に観測した）観測量Ｚ_ｋ，ｒを利用して１ステップ前に予測した状態推定値Ｘ_{ｋ｜ｋ-１}及び誤差共分散Ｐ_{ｋ｜ｋ-１}を更新する処理である。一方、予測処理とは、更新した状態推定値Ｘ_ｋ｜ｋ及び誤差共分散Ｐ_ｋ｜ｋから次のステップの状態推定値Ｘ_{ｋ+１｜ｋ}及び誤差共分散Ｐ_{ｋ+１｜ｋ}を予測する処理である。そして、カルマンフィルターは、この２つの処理を繰り返すことにより、尤もらしい状態推定値Ｘ_ｋを推定する。

まず、観測更新は、次式（６）〜（９）で定式化される。

なお、（６）〜（９）において、Ｓ_ｋは、上述した観測残差Δ_ｋの共分散である。Ｐ_{ｋ｜ｋ-１}は、時刻ｔ_ｋ-１において推定した時刻ｔ_ｋのシステムの状態推定値Ｘ_{ｋ｜ｋ−１}と、時刻ｔ_ｋの実際の状態量Ｘ_ｋとの差の共分散（誤差共分散）である。また、観測行列Ｈ_ｋは、状態推定値Ｘ_ｋの次元を観測量Ｚ_ｋの次元に変換するための変換行列として用いられる。観測行列Ｈ_ｋを１ステップ前に予測された状態推定値Ｘ_{ｋ｜ｋ-１}に作用させることで、観測量の予測値Ｚ_ｋ，eが得られる。さらに、Ｋ_ｋはカルマンゲインと呼ばれる補正係数である。カルマンゲインＫ_ｋは、予測された状態推定値Ｘ_{ｋ｜ｋ-１}を重視するか、Ｚ_ｋ，ｒとＺ_ｋ，eとの差Δ_ｋを重視するかを決定付けるパラメーター値として用いられる。カルマンゲインＫ_ｋは、観測誤差Ｒ_ｋの大きさによって決定付けられる。観測誤差Ｒ_ｋは、観測量Ｚ_ｋに含まれる想定誤差の大きさを表すパラメーター（想定誤差パラメーター）である。観測誤差Ｒ_ｋは、期待値（平均値）を“０”とする正規分布型のホワイトノイズと仮定して設定される。なお、Ｉは単位行列である。

式（８）は、Ｚ_ｋ，ｒとＺ_ｋ，eとの差Δ_ｋを用いて、状態推定値Ｘ_ｋ｜ｋを更新することを表している。

また、式（９）は、１ステップ前に予測された誤差共分散Ｐ_{ｋ｜ｋ-１}とカルマンゲインＫ_ｋと観測行列Ｈ_ｋとを用いて、誤差共分散Ｐ_ｋ｜ｋを更新することを表している。

一方、予測処理は、次式（１０）及び（１１）で定式化される。

なお、（１１）式において、Ｑ_ｋはシステム雑音と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。状態遷移行列Ｆ_ｋは、観測更新で更新された状態推定値Ｘ_ｋ|ｋから次のステップの状態推定値Ｘ_ｋ+１|ｋへの遷移を表す行列として定義される。

（３）式〜（１１）式にて、表される値等をまとめて、カルマンフィルターの内部状態変数と呼ぶ。

以上がカルマンフィルターの概要である。なお、カルマンフィルターは拡張カルマンフィルターであってもよい。

以上の本実施形態では、カルマンフィルターの演算処理を行う演算部１２０と、演算部１２０を制御する制御部１４０と、カルマンフィルターの演算処理の演算結果を記憶する記憶部１６０と、を含む。そして、演算部１２０は、演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、制御部１４０は、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。

固定小数点数とは、コンピューターにおける実数の近似値の表現方式の一つである。固定小数点数では整数部分に用いるビット数と小数部分に用いるビット数をあらかじめ固定して表現する。具体的な例を図２に示す。図２では、３２ビットの固定小数点数により、誤差共分散Ｐを表現しており、ＦＰが固定小数点である。

また、固定小数点位置とは、固定小数点の位置のことをいい、図２においては、ビット番号２５と２６の間が固定小数点位置となる。なお、固定小数点位置はビット番号０〜３１の範囲内である必要はなく、例えばビット番号１６〜４７の範囲内で調整するものであってもよい。

本実施形態の一態様では、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。よって、誤差共分散の固定小数点位置を調整しつつ、カルマンフィルターの演算処理を行うことが可能となる。

これにより、浮動小数点数によりカルマンフィルターの演算処理を行う場合に対して、少ない計算量で、演算処理を行うことができる。また、オーバーフローを発生させないように、固定小数点位置を調整して、演算することができる。

また、制御部１４０は、誤差共分散についての過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。

これにより、過去の誤差共分散の演算結果に基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。

また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。

上位ビットとは、誤差共分散を固定小数点数により表した場合のＭＳＢ（Most Significant Bit）側に位置する任意の数のビットのことである。例えば、図２においては、上位ビット数を４とし、ＨＢを上位ビットとしている。

これにより、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。

また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットにより表される値が、第一の閾値以上である場合には、決定処理として、誤差共分散の固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、右シフト量を演算部１２０に通知し、演算部１２０は、制御部１４０から右シフト量を通知された場合に、右シフト量に基づき、誤差共分散を算術右シフトしてもよい。

上位ビットにより表される値とは、具体的に図２においては、２進数で０００１、１０進数では１となる。

また、第一の閾値とは、算術右シフトを行うか否かを判断するための閾値である。

さらに、シフト量とは、算術シフトを行う際のビット移動量のことをいう。また、ＬＳＢ（Least Significant Bit）側に算術シフトすることを算術右シフトといい、ＭＳＢ側に算術シフトすることを算術左シフトという。シフト量の中で、算術右シフトをする際のシフト量のことを右シフト量といい、算術左シフトをする際のシフト量のことを左シフト量という。

これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第一の閾値を超えている場合には、誤差共分散を算術右シフトし、カルマンフィルターの演算処理中にオーバーフローが発生しないように固定小数点位置を調整すること等が可能になる。

具体例を図３（Ａ）、図３（Ｂ）に示す。図３（Ａ）においては、誤差共分散Ｐを浮動小数点数により表している。図３（Ａ）においては、固定小数点がＦＰ１であり、Ｐの整数部分がＩＰ１、小数部分がＤＰ１に相当する。さらに、第一の閾値を１０進数で３としている。この場合においては、上位ビットにより表される値が７であり、第一の閾値以上であるため、算術右シフトを行うことが決定される。なお、この例では、右シフト量を一律１と設定している。

一方、図３（Ｂ）には、算術右シフト後の誤差共分散Ｐを表している。算術右シフト後には、Ｐの整数部分がＩＰ２、小数部分がＤＰ２となる。その結果、整数部分がＩＰ１からＩＰ２へと拡大され、オーバーフローが発生しにくくなる。

また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットにより表される値が、第二の閾値以下である場合には、決定処理として、誤差共分散の固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、左シフト量を演算部に通知し、演算部１２０は、制御部１４０から左シフト量を通知された場合に、左シフト量に基づき、誤差共分散を算術左シフトしてもよい。

第二の閾値とは、算術左シフトを行うか否かを判断するための閾値である。

これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第二の閾値以下である場合には、誤差共分散を算術左シフトし、カルマンフィルターの演算処理の正確度を高めるように固定小数点位置を調整すること等が可能になる。

具体例を図３（Ａ）、図３（Ｂ）と同様に、図４（Ａ）、図４（Ｂ）に示す。図４（Ａ）においては、固定小数点がＦＰ３であり、Ｐの整数部分がＩＰ３、小数部分がＤＰ３に相当する。さらに、第二の閾値を１０進数で０としている。この場合においては、上位ビットにより表される値が０であり、第二の閾値以下であるため、算術左シフトを行うことが決定される。なお、この例では、左シフト量を一律１と設定している。

一方、図４（Ｂ）には、算術左シフト後の誤差共分散Ｐを表している。算術左シフト後には、Ｐの整数部分がＩＰ４、小数部分がＤＰ４となる。その結果、小数部分がＤＰ３からＤＰ４へと拡大され、より正確度の高い演算処理を行うことができるようになる。

また、制御部１４０は、過去の誤差共分散の正負判断を行い、過去の誤差共分散の正負に応じて、過去の誤差共分散に任意のビット列を加算し、ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。

任意のビット列とは、誤差共分散について演算処理を行う場合に、オーバーフローが発生するか否かを擬似的に判定するために用いるビット列のことをいう。具体例については後述する。

また、オーバーフローフラグとは、ステータスレジスターの中のフラグビットの一つであり、カルマンフィルターの演算において、オーバーフローが発生したか否かをビットの状態によって判別することができるフラグのことをいう。なお、オーバーフローフラグをゼロフラグなどの他のフラグビットに置き換えて、同様の機能を実現する実施形態も本発明に含まれるのものとする。

具体例を図５（Ａ）〜図８（Ｄ）を用いて説明する。

図５（Ａ）〜図６（Ｄ）は、誤差共分散Ｐの値が正である場合を示している。

図５（Ａ）は、固定小数点がＦＰ５である正の誤差共分散Ｐを示しており、図５（Ｂ）は任意のビット列Ｍを示している。このビット列Ｍは、誤差共分散が正の場合において、誤差共分散を算術右シフトして、整数部分を拡大させるか否かの判断をするために用いられる。本例では、誤差共分散Ｐの上位４ビットにより表される値が１０進数で３以上であれば、オーバーフローが発生する可能性が高いと仮定し、その際には算術右シフトを行い、誤差共分散Ｐの整数部分を拡大させる。そこで、この判断を行う際に、図５（Ｂ）に示すビット列Ｍを用い、整数部分を拡大する必要があるかを判断する。この判断は、図５（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算したビット列Ｐ＆Ｍを求め、この時、オーバーフローフラグＯＦが１であるか否かを判断することによって行われる。この例においては、オーバーフローフラグが１であるので、上位４ビットにより表される値が３以上であると判断し、右シフト量を１とする算術右シフトを行う。その様子を示しているのが図５（Ｄ）であり、固定小数点がＦＰ５からＦＰ６に移動している。これにより、整数部分が拡大し、オーバーフローが発生しにくくなっている。一方、上位４ビットにより表される値が３よりも小さい場合には、何もしない。

次に、図６（Ａ）は、固定小数点がＦＰ７である正の誤差共分散Ｐを示しており、図６（Ｂ）は任意のビット列Ｍを示している。このビット列Ｍは誤差共分散が正の場合において、誤差共分散を算術左シフトして、小数部分を拡大させるか否かの判断をするために用いられる。つまり、誤差共分散Ｐが正である場合、負である場合のそれぞれにおいて、算術右シフトをするか否かを判断するビット列と、算術左シフトをするか否かを判断するビット列の２種類のビット列をあらかじめ設定しておく。本例では、誤差共分散Ｐの上位４ビットにより表される値が１０進数で０であれば、演算処理の正確度が低下する可能性が高いと仮定し、その際には算術左シフトを行い、誤差共分散Ｐの小数部分を拡大させる。そこで、この判断を行う際に、図６（Ｂ）に示すビット列Ｍを用い、小数部分を拡大する必要があるかを判断する。この判断は、図６（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算したビット列Ｐ＆Ｍを求め、この時、オーバーフローフラグＯＦが０であるか否かを判断することによって行われる。この例においては、オーバーフローフラグが０であるので、上位４ビットにより表される値が０であると判断し、左シフト量を１とする算術左シフトを行う。その様子を示しているのが図６（Ｄ）であり、固定小数点がＦＰ７からＦＰ８に移動している。これにより、小数部分が拡大し、演算処理の正確度の向上を期待することができる。一方、上位４ビットにより表される値が０よりも大きい場合には、何もしない。

図７（Ａ）〜図８（Ｄ）は、誤差共分散Ｐの値が負である場合を示している。

誤差共分散Ｐの値が負である場合にも、同様に２種類のビット列を用意しておく。その一例として、図７（Ｂ）に示す任意のビット列Ｍは、Ｐの上位４ビットにより表される値が−３以下であるかを判断するために用いられる。一方、図８（Ｂ）に示す任意のビット列Ｍは、Ｐの上位４ビットにより表される値が−１であるかを判断するために用いられる。

例えば、図７（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算した結果、オーバーフローフラグが０であるので、上位４ビットにより表される値が−３以下であると判断し、右シフト量を１とする算術右シフトを行う。その様子を示しているのが図７（Ｄ）であり、固定小数点がＦＰ９からＦＰ１０に移動している。これにより整数部分が拡大される。一方、上位４ビットにより表される値が−３よりも小さい場合、すなわちオーバーフローフラグが１である場合には、何もしない。

同様に、図８（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算した結果、オーバーフローフラグが１であるので、上位４ビットにより表される値が−１であると判断し、左シフト量を１とする算術左シフトを行う。その様子を示しているのが図８（Ｄ）であり、固定小数点がＦＰ１１からＦＰ１２に移動している。これにより小数部分が拡大される。一方、上位４ビットにより表される値が−１よりも小さい場合、すなわちオーバーフローフラグが０である場合には、何もしない。

このように、誤差共分散の絶対値を求めずに、誤差共分散の正負の判断を行い、任意のビット列を加算して、その際のオーバーフローフラグの値に基づいて、誤差共分散の固定小数点位置を調整すること等が可能になる。

また、演算部１２０は、飽和演算命令により、誤差共分散の演算処理を行い、制御部１４０は、演算処理において、飽和が発生したと判断する場合には、誤差共分散の固定小数点位置を変更するためのシフト量を決定し、シフト量を演算部１２０に通知し、演算部１２０は、制御部１４０から通知されたシフト量に基づいて、演算処理前の誤差共分散を算術シフトし、固定小数点位置を変更してもよい。

飽和演算とは、サチュレート演算ともいい、数値の上限と下限が決まっており、演算結果がその値を超えた場合は上限値又は下限値になる演算をいう。

これにより、カルマンフィルターの演算処理を飽和演算により行い、演算処理中に飽和が発生したか否かによって、固定小数点位置を変更する否かを決定し、固定小数点位置を調整すること等が可能になる。

また、演算部１２０は、誤差共分散が行列で表される場合に、演算処理の中で、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、制御部１４０は、誤差共分散が行列で表される場合に、過去の誤差共分散行列の各要素に基づいて、現在の誤差共分散行列の各要素についての固定小数点位置の決定処理を行ってもよい。

行列の各要素の固定小数点位置が異なると、積和の度に固定小数点位置の整合性を取る必要があるため、一つの行列内では同じ固定小数点位置で表現することとする。

具体的には、誤差共分散が行列で表される場合というのは、観測量Ｚが行列である場合である。この場合には、（６）式、（９）式、（１１）式における誤差共分散Ｐも行列で表される。なお、それに伴い（４）式から（１１）式における各項（例えば、Ｘ、Ｈ、Ｓなど）も行列で表される。

ここで、状態推定値Ｘについての誤差共分散は、Ｘを計算した時の確からしさを意味する。

また、状態推定値Ｘが（１２）式で表すような行列であり、

Ｘについての誤差共分散Ｐが（１３）式で表すような行列である場合に、

誤差共分散の要素ｐ_ｉｊは、ｘ_ｉの値を計算した時のｘ_ｊの値の確からしさを表す。

本実施形態によれば、例えば、誤差共分散の要素の絶対値の中で最も大きい値ｐ_ｋｌの値に基づき、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。

また、制御部１４０は、誤差共分散が行列で表される場合に、過去の誤差共分散行列の対角要素に基づいて、現在の誤差共分散行列の各要素についての固定小数点位置の決定処理を行ってもよい。

対角要素は、行列の対格線上にある要素のことをいい、（１３）式の例でいうと、ｐ_ｉｉ等ことである。ｐ_ｉｉは、ｘ_ｉの値を計算した時のｘ_ｉ自身の確からしさ、いわゆる自己相関を表す。そのため、ｘ_ｉの値の確からしさを知りたい場合には最も有用な情報となる。

これにより、誤差共分散が行列で表される場合に、誤差共分散行列の対角要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。

また、演算部１２０は、誤差共分散及び状態推定値の予測処理と観測更新を行い、制御部１４０は、予測処理における誤差共分散の固定小数点位置の決定処理を行い、観測更新における誤差共分散の固定小数点位置の決定処理を行ってもよい。

これにより、観測更新と予測処理のそれぞれについて、誤差共分散の固定小数点位置を調整すること等が可能になる。

また、制御部１４０は、（１１）式に示すＰ＝ＦＰＦ^Ｔ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦ^Ｔ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行ってもよい。

内部状態変数の加算時において、固定小数点位置の余計な調整を行わなずに、そのまま加算を行うためには、加算する内部状態変数の固定小数点位置をあらかじめ揃えておく必要がある。

例えば、（１１）式のＰ＝ＦＰＦ^Ｔ＋Ｑの演算を考える。この場合、ＦＰＦ^ＴとＱの固定小数点位置が、左辺のＰと同じ固定小数点位置であれば、固定小数点位置の調整をせずに加算を行うことができる。つまり、Ｐと、ＦＰＦ^Ｔ、Ｑの固定小数点位置を全て同一にすればよい。

これにより、Ｐ＝ＦＰＦ^Ｔ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦ^Ｔ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＦＰＦ^Ｔ及びＱの固定小数点位置の調整すること等が可能になる。

また、制御部１４０は、（６）式に示すＳ＝ＨＰＨ^Ｔ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨ^Ｔ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行ってもよい。

例えば、（６）式のＳ＝ＨＰＨ^Ｔ＋Ｒの演算を考える。この場合、ＨＰＨ^ＴとＲの固定小数点位置が、左辺のＳと同じ固定小数点位置であれば、固定小数点位置の調整をせずに加算を行うことができる。つまり、Ｓと、ＨＰＨ^Ｔ、Ｒの固定小数点位置を全て同一にすればよい。さらに、ＨＰＨ^Ｔの固定小数点位置を、Ｐの固定小数点位置と同一にすれば、Ｐと、ＦＰＦ^Ｔ、ＨＰＨ^Ｔ、Ｑ、Ｓ、Ｒの固定小数点位置を全て同一にすることができ、演算量のさらなる削減を図ることができる。

これにより、Ｓ＝ＨＰＨ^Ｔ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨ^Ｔ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＨＰＨ^Ｔ及びＲの固定小数点位置の調整すること等が可能になる。

なお本実施形態の状態推定装置等は、プログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態の状態推定装置等が実現される。具体的には情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そしてＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。

３．処理の流れ
以下では、図９と図１０のフローチャートを用いて、本実施形態の処理の流れについて説明する。

３．１ 固定小数点位置を調整しながらカルマンフィルターの演算を行う処理の詳細
図９のフローチャートに示すように、まず、一定期間が経過しているかを判断する（Ｓ１）。一定期間が経過している場合には、固定小数点位置の調整を行う（Ｓ２）。固定小数点位置の調整後、または一定期間が経過していないと判断する場合には、カルマンフィルターの観測更新処理を行う（Ｓ３）。そして、最後に予測処理を行う（Ｓ４）。

以上の処理を一定周期で行う。

３．１．１ 固定小数点位置を調整する処理の詳細
図１０のフローチャートに示すように、前回の誤差共分散行列の各要素の絶対値のうち最大となる値を特定する（Ｓ１０）。次に、特定した値に上位４ビットにより表される値を特定する（Ｓ１１）。そして、この上位４ビットにより表される値が第一の閾値より大きいかを判断する（Ｓ１２）。第一の閾値以上であると判断する場合には、右シフト量を決定する（Ｓ１３）。一方、第一の閾値以下であると判断する場合には、上位４ビットにより表される値が第二の閾値以下であるかを判断する（Ｓ１４）。第二の閾値以下であると判断する場合には、左シフト量を決定する（Ｓ１５）。右シフト量又は左シフト量が決定された場合には、シフト量に基づいて誤差共分散Ｐを算術シフトする（Ｓ１６）。最後に、算術シフト後、または、上位ビットにより表される値が第二の閾値以下であると判断する場合には、ＦＰＦ^Ｔ、ＨＰＨ^Ｔ、Ｑ、Ｓ、Ｒの固定小数点位置を、全てＰの固定小数点位置と同一にする（Ｓ１７）。

以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また状態推定装置及びこれを含む電子機器の構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。

１００ 状態推定装置、１２０ 演算部、１４０ 制御部、１６０ 記憶部

Claims (14)

1. カルマンフィルターの演算処理を行う演算部と、
   前記演算部を制御する制御部と、
   前記カルマンフィルターの演算処理の演算結果を記憶する記憶部と、
   を含み、
   前記演算部は、
   前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、
   前記制御部は、
   過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とする状態推定装置。
2. 請求項１において、
   前記制御部は、
   前記誤差共分散についての過去の前記演算結果に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
3. 請求項２において、
   前記制御部は、
   過去の前記誤差共分散の絶対値における上位ビットに基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
4. 請求項３において、
   前記制御部は、
   過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第一の閾値以上である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、前記右シフト量を前記演算部に通知し、
   前記演算部は、
   前記制御部から通知された前記右シフト量に基づき、前記誤差共分散を算術右シフトすることを特徴とする状態推定装置。
5. 請求項３又は４において、
   前記制御部は、
   過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第二の閾値以下である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、前記左シフト量を前記演算部に通知し、
   前記演算部は、
   前記制御部から通知された前記左シフト量に基づき、前記誤差共分散を算術左シフトすることを特徴とする状態推定装置。
6. 請求項２において、
   前記制御部は、
   過去の前記誤差共分散の正負判断を行い、過去の前記誤差共分散の正負に応じて、過去の前記誤差共分散に任意のビット列を加算し、前記ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
7. 請求項２において、
   前記演算部は、
   飽和演算命令により、前記誤差共分散の前記演算処理を行い、
   前記制御部は、
   前記演算処理において、飽和が発生したと判断する場合には、前記誤差共分散の前記固定小数点位置を変更するためのシフト量を決定し、前記シフト量を演算部に通知し、
   前記演算部は、
   前記制御部から通知された前記シフト量に基づいて、前記演算処理前の前記誤差共分散を算術シフトし、前記固定小数点位置を変更することを特徴とする状態推定装置。
8. 請求項２乃至７のいずれかにおいて、
   前記演算部は、
   前記誤差共分散が行列で表される場合に、前記演算処理として、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、
   前記制御部は、
   過去の前記誤差共分散行列の各要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
9. 請求項８において、
   前記制御部は、
   過去の前記誤差共分散行列の対角要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
10. 請求項１乃至９のいずれかにおいて、
    前記演算部は、
    前記誤差共分散及び状態推定値の予測処理と観測更新を行い、
    前記制御部は、
    前記予測処理における前記誤差共分散の前記固定小数点位置の前記決定処理を行い、前記観測更新における前記誤差共分散の前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
11. 請求項１０において、
    前記制御部は、
    Ｐ＝ＦＰＦ^Ｔ＋Ｑの演算処理により、前記誤差共分散Ｐを求める場合に、ＦＰＦ^Ｔ及びＱの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行うことを特徴とする状態推定装置。
12. 請求項１０又は１１において、
    前記制御部は、
    Ｓ＝ＨＰＨ^Ｔ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨ^Ｔ及びＲの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行うことを特徴とする状態推定装置。
13. 請求項１乃至１２のいずれかに記載の状態推定装置を含むことを特徴とする電子機器。
14. カルマンフィルターの演算処理を行う演算部と、
    前記演算部を制御する制御部と、
    前記カルマンフィルターの演算処理の演算結果を記憶する記憶部として、
    コンピューターを機能させ、
    前記演算部は、
    前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、
    前記制御部は、
    過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とするプログラム。