状態推定装置、電子機器及びプログラム
【課題】 カルマンフィルターの演算において、固定小数点数を用い、固定小数点位置を調整しながら演算を行うことにより、計算量を抑えつつ、演算の破綻を回避できる状態推定装置等の提供。
【解決手段】 状態推定装置１００は、カルマンフィルターの演算処理を行う演算部１２０と、演算部１２０を制御する制御部１４０と、演算結果を記憶する記憶部１６０と、を含む。そして、演算部１２０は、演算処理の中で、固定小数点数として表現される誤差共分散の演算処理を行う。制御部１４０は、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。
【解決手段】 状態推定装置１００は、カルマンフィルターの演算処理を行う演算部１２０と、演算部１２０を制御する制御部１４０と、演算結果を記憶する記憶部１６０と、を含む。そして、演算部１２０は、演算処理の中で、固定小数点数として表現される誤差共分散の演算処理を行う。制御部１４０は、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、状態推定装置、電子機器及びプログラム等に関係する。
【背景技術】
【０００２】
カルマンフィルターにおいて、内部状態変数の演算処理を行う方法として、内部状態変数や観測量を固定小数点数により表現して演算を行う手法と、内部状態変数や観測量を浮動小数点数により表現して演算を行う手法が考えられる。
【０００３】
固定小数点数により表現して演算を行う手法では、小数点位置を固定的に決めておき、表現可能な範囲の値を用いて演算を行う。例えば、３２ビットにより表現される固定小数点数において、２の補数表現で符号ビットを考慮し、固定小数点位置がビット番号３０と３１の間であるとする場合には、［−１，＋１）の範囲の値が表現できる。この時、最小分解能は、１／２３１である。
【０００４】
また、適応フィルターの演算処理において、固定小数点数を用いる発明としては特許文献１に記載される従来技術がある。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００９―３０２８８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
カルマンフィルターにおいて、内部状態変数や観測量を固定小数点数により表現して演算を行う手法には、演算中にオーバーフローが発生し、演算結果が不定値となり、演算が破綻する場合があるという問題点がある。
【０００７】
一方、内部状態変数や観測量を浮動小数点数により表現して演算を行う手法では、演算を破綻させずに行うことはできるものの、計算量が増大してしまうという問題点がある。
【０００８】
本発明の幾つかの態様によれば、カルマンフィルターの演算において、固定小数点数を用い、固定小数点位置を調整しながら演算を行うことにより、計算量を抑えつつ、演算の破綻を回避できる状態推定装置、電子機器及びプログラム等を提供できる。
【課題を解決するための手段】
【０００９】
本発明の一態様は、カルマンフィルターの演算処理を行う演算部と、前記演算部を制御する制御部と、カルマンフィルターの演算処理の演算結果を記憶する記憶部と、を含み、前記演算部は、前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、前記制御部は、過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とする状態推定装置に関係する。
【００１０】
本発明の一態様では、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。よって、誤差共分散の固定小数点位置を調整しつつ、カルマンフィルターの演算処理を行うことが可能となる。
【００１１】
本発明の一態様では、前記制御部は、前記誤差共分散についての過去の前記演算結果に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。
【００１２】
これにより、過去の誤差共分散の演算結果に基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００１３】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における上位ビットに基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。
【００１４】
これにより、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００１５】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第一の閾値以上である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、前記右シフト量を前記演算部に通知し、前記演算部は、前記制御部から前記右シフト量を通知された場合に、前記右シフト量に基づき、前記誤差共分散を算術右シフトしてもよい。
【００１６】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第一の閾値を超えている場合には、誤差共分散を算術右シフトし、カルマンフィルターの演算処理中にオーバーフローが発生しないように固定小数点位置を調整すること等が可能になる。
【００１７】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第二の閾値以下である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、前記左シフト量を前記演算部に通知し、前記演算部は、前記制御部から前記左シフト量を通知された場合に、前記左シフト量に基づき、前記誤差共分散を算術左シフトしてもよい。
【００１８】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第二の閾値以下である場合には、誤差共分散を算術左シフトし、カルマンフィルターの演算処理の正確度を高めるように固定小数点位置を調整すること等が可能になる。
【００１９】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の正負判断を行い、過去の前記誤差共分散の正負に応じて、過去の前記誤差共分散に任意のビット列を加算し、前記ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。
【００２０】
これにより、誤差共分散の絶対値を求めずに、誤差共分散の正負の判断を行い、任意のビット列を加算して、その際のオーバーフローフラグの値に基づいて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【００２１】
また、本発明の一態様では、前記演算部は、飽和演算命令により、前記誤差共分散の前記演算処理を行い、前記制御部は、前記演算処理において、飽和が発生したと判断する場合には、前記誤差共分散の前記固定小数点位置を変更するためのシフト量を決定し、前記シフト量を演算部に通知し、前記演算部は、前記制御部から通知された前記シフト量に基づいて、前記演算処理前の前記誤差共分散を算術シフトし、前記固定小数点位置を変更してもよい。
【００２２】
これにより、カルマンフィルターの演算処理を飽和演算により行い、演算処理中に飽和が発生したか否かによって、固定小数点位置を変更する否かを決定し、固定小数点位置を調整すること等が可能になる。
【００２３】
また、本発明の一態様では、前記演算部は、前記誤差共分散が行列で表される場合に、前記演算処理の中で、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、前記制御部は、前記誤差共分散が行列で表される場合に、過去の前記誤差共分散行列の各要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うってもよい。
【００２４】
これにより、誤差共分散が行列で表される場合に、誤差共分散行列の各要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【００２５】
また、本発明の一態様では、前記制御部は、前記誤差共分散が行列で表される場合に、過去の前記誤差共分散行列の対角要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行ってもよい。
【００２６】
これにより、誤差共分散が行列で表される場合に、誤差共分散行列の対角要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【００２７】
また、本発明の一態様では、前記演算部は、前記誤差共分散及び状態推定値の予測処理と観測更新を行い、前記制御部は、前記予測処理における前記誤差共分散の前記固定小数点位置の前記決定処理を行い、前記観測更新における前記誤差共分散の前記固定小数点位置の前記決定処理を行ってもよい。
【００２８】
これにより、観測更新と予測処理のそれぞれについて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【００２９】
また、本発明の一態様では、前記制御部は、Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、前記誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行ってもよい。
【００３０】
これにより、Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＦＰＦＴ及びＱの固定小数点位置の調整すること等が可能になる。
【００３１】
また、本発明の一態様では、前記制御部は、Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行ってもよい。
【００３２】
これにより、Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＨＰＨＴ及びＲの固定小数点位置の調整すること等が可能になる。
【００３３】
また、本発明の他の態様では、前記状態推定装置を含むことを特徴とする電子機器に関係する。
【００３４】
また、本発明の他の態様では、上記各部としてコンピューターを機能させるプログラム、又は該プログラムを記憶したコンピューター読み取り可能な情報記憶媒体に関係する。
【図面の簡単な説明】
【００３５】
【図１】本実施形態の状態推定装置の構成例。
【図２】固定小数点と上位ビットの説明図。
【図３】図３（Ａ）、図３（Ｂ）は、算術右シフトにより固定小数点位置を変更する手法の説明図。
【図４】図４（Ａ）、図４（Ｂ）は、算術左シフトにより固定小数点位置を変更する手法の説明図。
【図５】図５（Ａ）〜図５（Ｄ）は、Ｐが正の値である場合に、第１のビット列を用いて固定小数点位置を変更する手法の説明図。
【図６】図６（Ａ）〜図６（Ｄ）は、Ｐが正の値である場合に、第２のビット列を用いて固定小数点位置を変更する手法の説明図。
【図７】図７（Ａ）〜図７（Ｄ）は、Ｐが負の値である場合に、第３のビット列を用いて固定小数点位置を変更する手法の説明図。
【図８】図８（Ａ）〜図８（Ｄ）は、Ｐが負の値である場合に、第４のビット列を用いて固定小数点位置を変更する手法の説明図。
【図９】固定小数点位置を調整しながらカルマンフィルターの演算を行う手法のフローチャート。
【図１０】固定小数点位置を調整する手法のフローチャート。
【発明を実施するための形態】
【００３６】
以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。
【００３７】
１．概要
本実施形態では、カルマンフィルターの演算において、観測量や内部状態変数などを固定小数点数として扱い、演算を行う。これにより、内部状態変数等を浮動小数点数として演算を行う場合と比較して、計算量の削減を図ることができる。
【００３８】
また、本実施形態では、カルマンフィルターの演算において、オーバーフローが発生しないように、誤差共分散の固定小数点位置を動的に変更する。誤差共分散は、カルマンフィルターの演算の中で、最もダイナミックレンジが広いと考えられるためである。これにより、誤差共分散の演算中にオーバーフローが発生し、演算結果が不定値となり、演算が破綻することを防ぐことができる。
【００３９】
さらに、本実施形態では、誤差共分散以外の内部状態変数の一部又は全てについて、固定小数点位置を誤差共分散の固定小数点位置と一致させる。これにより、状態推定値や観測残差等の内部状態変数の固定小数点位置を、計算量を増加させずに、オーバーフローが発生しないように調整することができる。
【００４０】
以下、本実施形態のシステム構成例を説明し、その後、フローチャートを用いて本実施形態の処理の詳細について説明する。
【００４１】
２．システム構成例
次に、図１に本実施形態の状態推定装置及びこれを含む電子機器の構成例を示す。
【００４２】
状態推定装置１００は、演算部１２０と、制御部１４０と、記憶部１６０と、を含む。また、状態推定装置１００を含む電子機器の例としては、センサーや外部制御部や操作部や表示部などを含む携帯端末などが挙げられる。なお、状態推定装置１００及びこれを含む電子機器は、図１の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００４３】
次に各部で行われる処理について説明する。
【００４４】
演算部１２０は、カルマンフィルターの演算処理を行う。制御部１４０は、演算部１２０を制御する。記憶部１６０は、カルマンフィルターの演算処理の演算結果を記憶する。
【００４５】
ここで、カルマンフィルターの概要を簡単に説明する。
【００４６】
カルマンフィルターは、直接観測不可能な値を、観測可能な値を用いて精度良く推定する際に用いられる状態推定器である。
【００４７】
例えば、ある時刻ｔｋにおいて、あるシステムの観測不可能な状態量Ｘｋを求めたい場合を考える。Ｘｋは直接観測できない値であるが、逆にＸｋを用いれば、観測可能な値である観測量Ｚｋを、（１）式で表せることが分かっている。
【００４８】
【数１】
【００４９】
（１）式において、ｋはステップ数、Ｘｋは時刻ｔｋにおけるシステムの状態推定値、Ｚｋは時刻ｔｋにおいて入力される観測量、Ｈｋは観測行列、Ｖｋは観測雑音を示している。なお、Ｈｋは物理法則や測定等によって予め確定した行列である。（１）式を観測方程式という。
【００５０】
また、時刻ｔｋにおけるシステムの状態量Ｘｋは、１ステップ前の時刻ｔｋ-１の状態量Ｘｋ-１をもとに、次のように表現されることが分かっているとする。
【００５１】
【数２】
【００５２】
（２）式において、Ｘｋ-１は時刻ｔｋ-１におけるシステムの状態量、Ｆｋは状態遷移行列、Ｂｋは駆動行列、Ｕｋは制御入力、Ｗｋはシステム雑音（白色雑音）を示している。なお、Ｆｋ及びＢｋは、物理法則や測定等によって予め確定した行列である。（２）式を状態方程式という。
【００５３】
ここで、（１）式に観測により得られた観測量Ｚｋを代入すれば、真の状態量Ｘｋが求められるようにも思えるが、観測雑音Ｖｋが不明であるため、（１）式から直接に真の状態量Ｘｋを求めることはできない。
【００５４】
そこで、まず、（２）式において、システム雑音Ｗｋがないと仮定した場合を考え、その場合の（２）式に相当する（３）式により、状態推定値Ｘｋ|ｋ-１を求める。
【００５５】
【数３】
【００５６】
（３）式において、Ｘｋ|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値であり、Ｘｋ-１|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋ-１のシステムの状態推定値である。
【００５７】
次に、（１）式において、観測誤差がないと仮定した場合を考え、その場合の（１）式に相当する（４）式により、観測誤差がないと仮定した場合の観測量の予測値Ｚｋ，eを求める。（４）式のＸｋ|ｋ-１は（３）式により求められたものである。
【００５８】
【数４】
【００５９】
そして、実際に観測により得られた観測量Ｚｋ，ｒとＺｋ，eとの差をとる。
【００６０】
【数５】
【００６１】
Δｋを観測残差という。観測残差は、以下の３値に起因する値である。一つ目の値は、雑音がないとした場合における、状態推定値から求めた観測量の推定値（観測推定量）と、実際の観測量との誤差である。そして、二つ目の値は、観測雑音Ｖｋであり、三つ目の値は、システム雑音Ｗｋである。
【００６２】
さらに、観測残差Δｋの共分散Ｓｋを算出する。観測残差Δｋの共分散Ｓｋは、システムの状態推定値Ｘｋから求められた観測推定量の確からしさと、観測雑音とシステム雑音がどの程度含まれているかを表すものである。つまり、観測残差Δｋの共分散Ｓｋをできるだけ小さくすることができれば、観測雑音とシステム雑音を抑圧しつつ、観測推定量を確からしいものにすることができる。さらに、観測推定量の正確性が担保されれば、観測推定量を求める際に参照された状態推定値の正確さも期待できる。
【００６３】
したがって、観測残差Δｋの共分散Ｓｋをできるだけ小さくするように制御すれば、システムの状態推定値も確からしいものとなる。
【００６４】
カルマンフィルターでは、以上の原理を基に、観測更新と予測処理との２つの処理を繰り返し行う。観測更新とは、外部から与えられる（実際に観測した）観測量Ｚｋ，ｒを利用して１ステップ前に予測した状態推定値Ｘｋ｜ｋ-１及び誤差共分散Ｐｋ｜ｋ-１を更新する処理である。一方、予測処理とは、更新した状態推定値Ｘｋ｜ｋ及び誤差共分散Ｐｋ｜ｋから次のステップの状態推定値Ｘｋ+１｜ｋ及び誤差共分散Ｐｋ+１｜ｋを予測する処理である。そして、カルマンフィルターは、この２つの処理を繰り返すことにより、尤もらしい状態推定値Ｘｋを推定する。
【００６５】
まず、観測更新は、次式（６）〜（９）で定式化される。
【００６６】
【数６】
【００６７】
【数７】
【００６８】
【数８】
【００６９】
【数９】
【００７０】
なお、（６）〜（９）において、Ｓｋは、上述した観測残差Δｋの共分散である。Ｐｋ｜ｋ-１は、時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値Ｘｋ｜ｋ−１と、時刻ｔｋの実際の状態量Ｘｋとの差の共分散（誤差共分散）である。また、観測行列Ｈｋは、状態推定値Ｘｋの次元を観測量Ｚｋの次元に変換するための変換行列として用いられる。観測行列Ｈｋを１ステップ前に予測された状態推定値Ｘｋ｜ｋ-１に作用させることで、観測量の予測値Ｚｋ，eが得られる。さらに、Ｋｋはカルマンゲインと呼ばれる補正係数である。カルマンゲインＫｋは、予測された状態推定値Ｘｋ｜ｋ-１を重視するか、Ｚｋ，ｒとＺｋ，eとの差Δｋを重視するかを決定付けるパラメーター値として用いられる。カルマンゲインＫｋは、観測誤差Ｒｋの大きさによって決定付けられる。観測誤差Ｒｋは、観測量Ｚｋに含まれる想定誤差の大きさを表すパラメーター（想定誤差パラメーター）である。観測誤差Ｒｋは、期待値（平均値）を“０”とする正規分布型のホワイトノイズと仮定して設定される。なお、Ｉは単位行列である。
【００７１】
式（８）は、Ｚｋ，ｒとＺｋ，eとの差Δｋを用いて、状態推定値Ｘｋ｜ｋを更新することを表している。
【００７２】
また、式（９）は、１ステップ前に予測された誤差共分散Ｐｋ｜ｋ-１とカルマンゲインＫｋと観測行列Ｈｋとを用いて、誤差共分散Ｐｋ｜ｋを更新することを表している。
【００７３】
一方、予測処理は、次式（１０）及び（１１）で定式化される。
【００７４】
【数１０】
【００７５】
【数１１】
【００７６】
なお、（１１）式において、Ｑｋはシステム雑音と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。状態遷移行列Ｆｋは、観測更新で更新された状態推定値Ｘｋ|ｋから次のステップの状態推定値Ｘｋ+１|ｋへの遷移を表す行列として定義される。
【００７７】
（３）式〜（１１）式にて、表される値等をまとめて、カルマンフィルターの内部状態変数と呼ぶ。
【００７８】
以上がカルマンフィルターの概要である。なお、カルマンフィルターは拡張カルマンフィルターであってもよい。
【００７９】
以上の本実施形態では、カルマンフィルターの演算処理を行う演算部１２０と、演算部１２０を制御する制御部１４０と、カルマンフィルターの演算処理の演算結果を記憶する記憶部１６０と、を含む。そして、演算部１２０は、演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、制御部１４０は、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。
【００８０】
固定小数点数とは、コンピューターにおける実数の近似値の表現方式の一つである。固定小数点数では整数部分に用いるビット数と小数部分に用いるビット数をあらかじめ固定して表現する。具体的な例を図２に示す。図２では、３２ビットの固定小数点数により、誤差共分散Ｐを表現しており、ＦＰが固定小数点である。
【００８１】
また、固定小数点位置とは、固定小数点の位置のことをいい、図２においては、ビット番号２５と２６の間が固定小数点位置となる。なお、固定小数点位置はビット番号０〜３１の範囲内である必要はなく、例えばビット番号１６〜４７の範囲内で調整するものであってもよい。
【００８２】
本実施形態の一態様では、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。よって、誤差共分散の固定小数点位置を調整しつつ、カルマンフィルターの演算処理を行うことが可能となる。
【００８３】
これにより、浮動小数点数によりカルマンフィルターの演算処理を行う場合に対して、少ない計算量で、演算処理を行うことができる。また、オーバーフローを発生させないように、固定小数点位置を調整して、演算することができる。
【００８４】
また、制御部１４０は、誤差共分散についての過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。
【００８５】
これにより、過去の誤差共分散の演算結果に基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００８６】
また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。
【００８７】
上位ビットとは、誤差共分散を固定小数点数により表した場合のＭＳＢ（Most Significant Bit）側に位置する任意の数のビットのことである。例えば、図２においては、上位ビット数を４とし、ＨＢを上位ビットとしている。
【００８８】
これにより、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００８９】
また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットにより表される値が、第一の閾値以上である場合には、決定処理として、誤差共分散の固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、右シフト量を演算部１２０に通知し、演算部１２０は、制御部１４０から右シフト量を通知された場合に、右シフト量に基づき、誤差共分散を算術右シフトしてもよい。
【００９０】
上位ビットにより表される値とは、具体的に図２においては、２進数で０００１、１０進数では１となる。
【００９１】
また、第一の閾値とは、算術右シフトを行うか否かを判断するための閾値である。
【００９２】
さらに、シフト量とは、算術シフトを行う際のビット移動量のことをいう。また、ＬＳＢ（Least Significant Bit）側に算術シフトすることを算術右シフトといい、ＭＳＢ側に算術シフトすることを算術左シフトという。シフト量の中で、算術右シフトをする際のシフト量のことを右シフト量といい、算術左シフトをする際のシフト量のことを左シフト量という。
【００９３】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第一の閾値を超えている場合には、誤差共分散を算術右シフトし、カルマンフィルターの演算処理中にオーバーフローが発生しないように固定小数点位置を調整すること等が可能になる。
【００９４】
具体例を図３（Ａ）、図３（Ｂ）に示す。図３（Ａ）においては、誤差共分散Ｐを浮動小数点数により表している。図３（Ａ）においては、固定小数点がＦＰ１であり、Ｐの整数部分がＩＰ１、小数部分がＤＰ１に相当する。さらに、第一の閾値を１０進数で３としている。この場合においては、上位ビットにより表される値が７であり、第一の閾値以上であるため、算術右シフトを行うことが決定される。なお、この例では、右シフト量を一律１と設定している。
【００９５】
一方、図３（Ｂ）には、算術右シフト後の誤差共分散Ｐを表している。算術右シフト後には、Ｐの整数部分がＩＰ２、小数部分がＤＰ２となる。その結果、整数部分がＩＰ１からＩＰ２へと拡大され、オーバーフローが発生しにくくなる。
【００９６】
また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットにより表される値が、第二の閾値以下である場合には、決定処理として、誤差共分散の固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、左シフト量を演算部に通知し、演算部１２０は、制御部１４０から左シフト量を通知された場合に、左シフト量に基づき、誤差共分散を算術左シフトしてもよい。
【００９７】
第二の閾値とは、算術左シフトを行うか否かを判断するための閾値である。
【００９８】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第二の閾値以下である場合には、誤差共分散を算術左シフトし、カルマンフィルターの演算処理の正確度を高めるように固定小数点位置を調整すること等が可能になる。
【００９９】
具体例を図３（Ａ）、図３（Ｂ）と同様に、図４（Ａ）、図４（Ｂ）に示す。図４（Ａ）においては、固定小数点がＦＰ３であり、Ｐの整数部分がＩＰ３、小数部分がＤＰ３に相当する。さらに、第二の閾値を１０進数で０としている。この場合においては、上位ビットにより表される値が０であり、第二の閾値以下であるため、算術左シフトを行うことが決定される。なお、この例では、左シフト量を一律１と設定している。
【０１００】
一方、図４（Ｂ）には、算術左シフト後の誤差共分散Ｐを表している。算術左シフト後には、Ｐの整数部分がＩＰ４、小数部分がＤＰ４となる。その結果、小数部分がＤＰ３からＤＰ４へと拡大され、より正確度の高い演算処理を行うことができるようになる。
【０１０１】
また、制御部１４０は、過去の誤差共分散の正負判断を行い、過去の誤差共分散の正負に応じて、過去の誤差共分散に任意のビット列を加算し、ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。
【０１０２】
任意のビット列とは、誤差共分散について演算処理を行う場合に、オーバーフローが発生するか否かを擬似的に判定するために用いるビット列のことをいう。具体例については後述する。
【０１０３】
また、オーバーフローフラグとは、ステータスレジスターの中のフラグビットの一つであり、カルマンフィルターの演算において、オーバーフローが発生したか否かをビットの状態によって判別することができるフラグのことをいう。なお、オーバーフローフラグをゼロフラグなどの他のフラグビットに置き換えて、同様の機能を実現する実施形態も本発明に含まれるのものとする。
【０１０４】
具体例を図５（Ａ）〜図８（Ｄ）を用いて説明する。
【０１０５】
図５（Ａ）〜図６（Ｄ）は、誤差共分散Ｐの値が正である場合を示している。
【０１０６】
図５（Ａ）は、固定小数点がＦＰ５である正の誤差共分散Ｐを示しており、図５（Ｂ）は任意のビット列Ｍを示している。このビット列Ｍは、誤差共分散が正の場合において、誤差共分散を算術右シフトして、整数部分を拡大させるか否かの判断をするために用いられる。本例では、誤差共分散Ｐの上位４ビットにより表される値が１０進数で３以上であれば、オーバーフローが発生する可能性が高いと仮定し、その際には算術右シフトを行い、誤差共分散Ｐの整数部分を拡大させる。そこで、この判断を行う際に、図５（Ｂ）に示すビット列Ｍを用い、整数部分を拡大する必要があるかを判断する。この判断は、図５（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算したビット列Ｐ＆Ｍを求め、この時、オーバーフローフラグＯＦが１であるか否かを判断することによって行われる。この例においては、オーバーフローフラグが１であるので、上位４ビットにより表される値が３以上であると判断し、右シフト量を１とする算術右シフトを行う。その様子を示しているのが図５（Ｄ）であり、固定小数点がＦＰ５からＦＰ６に移動している。これにより、整数部分が拡大し、オーバーフローが発生しにくくなっている。一方、上位４ビットにより表される値が３よりも小さい場合には、何もしない。
【０１０７】
次に、図６（Ａ）は、固定小数点がＦＰ７である正の誤差共分散Ｐを示しており、図６（Ｂ）は任意のビット列Ｍを示している。このビット列Ｍは誤差共分散が正の場合において、誤差共分散を算術左シフトして、小数部分を拡大させるか否かの判断をするために用いられる。つまり、誤差共分散Ｐが正である場合、負である場合のそれぞれにおいて、算術右シフトをするか否かを判断するビット列と、算術左シフトをするか否かを判断するビット列の２種類のビット列をあらかじめ設定しておく。本例では、誤差共分散Ｐの上位４ビットにより表される値が１０進数で０であれば、演算処理の正確度が低下する可能性が高いと仮定し、その際には算術左シフトを行い、誤差共分散Ｐの小数部分を拡大させる。そこで、この判断を行う際に、図６（Ｂ）に示すビット列Ｍを用い、小数部分を拡大する必要があるかを判断する。この判断は、図６（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算したビット列Ｐ＆Ｍを求め、この時、オーバーフローフラグＯＦが０であるか否かを判断することによって行われる。この例においては、オーバーフローフラグが０であるので、上位４ビットにより表される値が０であると判断し、左シフト量を１とする算術左シフトを行う。その様子を示しているのが図６（Ｄ）であり、固定小数点がＦＰ７からＦＰ８に移動している。これにより、小数部分が拡大し、演算処理の正確度の向上を期待することができる。一方、上位４ビットにより表される値が０よりも大きい場合には、何もしない。
【０１０８】
図７（Ａ）〜図８（Ｄ）は、誤差共分散Ｐの値が負である場合を示している。
【０１０９】
誤差共分散Ｐの値が負である場合にも、同様に２種類のビット列を用意しておく。その一例として、図７（Ｂ）に示す任意のビット列Ｍは、Ｐの上位４ビットにより表される値が−３以下であるかを判断するために用いられる。一方、図８（Ｂ）に示す任意のビット列Ｍは、Ｐの上位４ビットにより表される値が−１であるかを判断するために用いられる。
【０１１０】
例えば、図７（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算した結果、オーバーフローフラグが０であるので、上位４ビットにより表される値が−３以下であると判断し、右シフト量を１とする算術右シフトを行う。その様子を示しているのが図７（Ｄ）であり、固定小数点がＦＰ９からＦＰ１０に移動している。これにより整数部分が拡大される。一方、上位４ビットにより表される値が−３よりも小さい場合、すなわちオーバーフローフラグが１である場合には、何もしない。
【０１１１】
同様に、図８（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算した結果、オーバーフローフラグが１であるので、上位４ビットにより表される値が−１であると判断し、左シフト量を１とする算術左シフトを行う。その様子を示しているのが図８（Ｄ）であり、固定小数点がＦＰ１１からＦＰ１２に移動している。これにより小数部分が拡大される。一方、上位４ビットにより表される値が−１よりも小さい場合、すなわちオーバーフローフラグが０である場合には、何もしない。
【０１１２】
このように、誤差共分散の絶対値を求めずに、誤差共分散の正負の判断を行い、任意のビット列を加算して、その際のオーバーフローフラグの値に基づいて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【０１１３】
また、演算部１２０は、飽和演算命令により、誤差共分散の演算処理を行い、制御部１４０は、演算処理において、飽和が発生したと判断する場合には、誤差共分散の固定小数点位置を変更するためのシフト量を決定し、シフト量を演算部１２０に通知し、演算部１２０は、制御部１４０から通知されたシフト量に基づいて、演算処理前の誤差共分散を算術シフトし、固定小数点位置を変更してもよい。
【０１１４】
飽和演算とは、サチュレート演算ともいい、数値の上限と下限が決まっており、演算結果がその値を超えた場合は上限値又は下限値になる演算をいう。
【０１１５】
これにより、カルマンフィルターの演算処理を飽和演算により行い、演算処理中に飽和が発生したか否かによって、固定小数点位置を変更する否かを決定し、固定小数点位置を調整すること等が可能になる。
【０１１６】
また、演算部１２０は、誤差共分散が行列で表される場合に、演算処理の中で、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、制御部１４０は、誤差共分散が行列で表される場合に、過去の誤差共分散行列の各要素に基づいて、現在の誤差共分散行列の各要素についての固定小数点位置の決定処理を行ってもよい。
【０１１７】
行列の各要素の固定小数点位置が異なると、積和の度に固定小数点位置の整合性を取る必要があるため、一つの行列内では同じ固定小数点位置で表現することとする。
【０１１８】
具体的には、誤差共分散が行列で表される場合というのは、観測量Ｚが行列である場合である。この場合には、（６）式、（９）式、（１１）式における誤差共分散Ｐも行列で表される。なお、それに伴い（４）式から（１１）式における各項（例えば、Ｘ、Ｈ、Ｓなど）も行列で表される。
【０１１９】
ここで、状態推定値Ｘについての誤差共分散は、Ｘを計算した時の確からしさを意味する。
【０１２０】
また、状態推定値Ｘが（１２）式で表すような行列であり、
【０１２１】
【数１２】
【０１２２】
Ｘについての誤差共分散Ｐが（１３）式で表すような行列である場合に、
【０１２３】
【数１３】
【０１２４】
誤差共分散の要素ｐｉｊは、ｘｉの値を計算した時のｘｊの値の確からしさを表す。
【０１２５】
本実施形態によれば、例えば、誤差共分散の要素の絶対値の中で最も大きい値ｐｋｌの値に基づき、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【０１２６】
また、制御部１４０は、誤差共分散が行列で表される場合に、過去の誤差共分散行列の対角要素に基づいて、現在の誤差共分散行列の各要素についての固定小数点位置の決定処理を行ってもよい。
【０１２７】
対角要素は、行列の対格線上にある要素のことをいい、（１３）式の例でいうと、ｐｉｉ等ことである。ｐｉｉは、ｘｉの値を計算した時のｘｉ自身の確からしさ、いわゆる自己相関を表す。そのため、ｘｉの値の確からしさを知りたい場合には最も有用な情報となる。
【０１２８】
これにより、誤差共分散が行列で表される場合に、誤差共分散行列の対角要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【０１２９】
また、演算部１２０は、誤差共分散及び状態推定値の予測処理と観測更新を行い、制御部１４０は、予測処理における誤差共分散の固定小数点位置の決定処理を行い、観測更新における誤差共分散の固定小数点位置の決定処理を行ってもよい。
【０１３０】
これにより、観測更新と予測処理のそれぞれについて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【０１３１】
また、制御部１４０は、（１１）式に示すＰ＝ＦＰＦＴ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行ってもよい。
【０１３２】
内部状態変数の加算時において、固定小数点位置の余計な調整を行わなずに、そのまま加算を行うためには、加算する内部状態変数の固定小数点位置をあらかじめ揃えておく必要がある。
【０１３３】
例えば、（１１）式のＰ＝ＦＰＦＴ＋Ｑの演算を考える。この場合、ＦＰＦＴとＱの固定小数点位置が、左辺のＰと同じ固定小数点位置であれば、固定小数点位置の調整をせずに加算を行うことができる。つまり、Ｐと、ＦＰＦＴ、Ｑの固定小数点位置を全て同一にすればよい。
【０１３４】
これにより、Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＦＰＦＴ及びＱの固定小数点位置の調整すること等が可能になる。
【０１３５】
また、制御部１４０は、（６）式に示すＳ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行ってもよい。
【０１３６】
例えば、（６）式のＳ＝ＨＰＨＴ＋Ｒの演算を考える。この場合、ＨＰＨＴとＲの固定小数点位置が、左辺のＳと同じ固定小数点位置であれば、固定小数点位置の調整をせずに加算を行うことができる。つまり、Ｓと、ＨＰＨＴ、Ｒの固定小数点位置を全て同一にすればよい。さらに、ＨＰＨＴの固定小数点位置を、Ｐの固定小数点位置と同一にすれば、Ｐと、ＦＰＦＴ、ＨＰＨＴ、Ｑ、Ｓ、Ｒの固定小数点位置を全て同一にすることができ、演算量のさらなる削減を図ることができる。
【０１３７】
これにより、Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＨＰＨＴ及びＲの固定小数点位置の調整すること等が可能になる。
【０１３８】
なお本実施形態の状態推定装置等は、プログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態の状態推定装置等が実現される。具体的には情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そしてＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。
【０１３９】
３．処理の流れ
以下では、図９と図１０のフローチャートを用いて、本実施形態の処理の流れについて説明する。
【０１４０】
３．１ 固定小数点位置を調整しながらカルマンフィルターの演算を行う処理の詳細
図９のフローチャートに示すように、まず、一定期間が経過しているかを判断する（Ｓ１）。一定期間が経過している場合には、固定小数点位置の調整を行う（Ｓ２）。固定小数点位置の調整後、または一定期間が経過していないと判断する場合には、カルマンフィルターの観測更新処理を行う（Ｓ３）。そして、最後に予測処理を行う（Ｓ４）。
【０１４１】
以上の処理を一定周期で行う。
【０１４２】
３．１．１ 固定小数点位置を調整する処理の詳細
図１０のフローチャートに示すように、前回の誤差共分散行列の各要素の絶対値のうち最大となる値を特定する（Ｓ１０）。次に、特定した値に上位４ビットにより表される値を特定する（Ｓ１１）。そして、この上位４ビットにより表される値が第一の閾値より大きいかを判断する（Ｓ１２）。第一の閾値以上であると判断する場合には、右シフト量を決定する（Ｓ１３）。一方、第一の閾値以下であると判断する場合には、上位４ビットにより表される値が第二の閾値以下であるかを判断する（Ｓ１４）。第二の閾値以下であると判断する場合には、左シフト量を決定する（Ｓ１５）。右シフト量又は左シフト量が決定された場合には、シフト量に基づいて誤差共分散Ｐを算術シフトする（Ｓ１６）。最後に、算術シフト後、または、上位ビットにより表される値が第二の閾値以下であると判断する場合には、ＦＰＦＴ、ＨＰＨＴ、Ｑ、Ｓ、Ｒの固定小数点位置を、全てＰの固定小数点位置と同一にする（Ｓ１７）。
【０１４３】
以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また状態推定装置及びこれを含む電子機器の構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１４４】
１００ 状態推定装置、１２０ 演算部、１４０ 制御部、１６０ 記憶部
【技術分野】
【０００１】
本発明は、状態推定装置、電子機器及びプログラム等に関係する。
【背景技術】
【０００２】
カルマンフィルターにおいて、内部状態変数の演算処理を行う方法として、内部状態変数や観測量を固定小数点数により表現して演算を行う手法と、内部状態変数や観測量を浮動小数点数により表現して演算を行う手法が考えられる。
【０００３】
固定小数点数により表現して演算を行う手法では、小数点位置を固定的に決めておき、表現可能な範囲の値を用いて演算を行う。例えば、３２ビットにより表現される固定小数点数において、２の補数表現で符号ビットを考慮し、固定小数点位置がビット番号３０と３１の間であるとする場合には、［−１，＋１）の範囲の値が表現できる。この時、最小分解能は、１／２３１である。
【０００４】
また、適応フィルターの演算処理において、固定小数点数を用いる発明としては特許文献１に記載される従来技術がある。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００９―３０２８８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
カルマンフィルターにおいて、内部状態変数や観測量を固定小数点数により表現して演算を行う手法には、演算中にオーバーフローが発生し、演算結果が不定値となり、演算が破綻する場合があるという問題点がある。
【０００７】
一方、内部状態変数や観測量を浮動小数点数により表現して演算を行う手法では、演算を破綻させずに行うことはできるものの、計算量が増大してしまうという問題点がある。
【０００８】
本発明の幾つかの態様によれば、カルマンフィルターの演算において、固定小数点数を用い、固定小数点位置を調整しながら演算を行うことにより、計算量を抑えつつ、演算の破綻を回避できる状態推定装置、電子機器及びプログラム等を提供できる。
【課題を解決するための手段】
【０００９】
本発明の一態様は、カルマンフィルターの演算処理を行う演算部と、前記演算部を制御する制御部と、カルマンフィルターの演算処理の演算結果を記憶する記憶部と、を含み、前記演算部は、前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、前記制御部は、過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とする状態推定装置に関係する。
【００１０】
本発明の一態様では、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。よって、誤差共分散の固定小数点位置を調整しつつ、カルマンフィルターの演算処理を行うことが可能となる。
【００１１】
本発明の一態様では、前記制御部は、前記誤差共分散についての過去の前記演算結果に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。
【００１２】
これにより、過去の誤差共分散の演算結果に基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００１３】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における上位ビットに基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。
【００１４】
これにより、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００１５】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第一の閾値以上である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、前記右シフト量を前記演算部に通知し、前記演算部は、前記制御部から前記右シフト量を通知された場合に、前記右シフト量に基づき、前記誤差共分散を算術右シフトしてもよい。
【００１６】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第一の閾値を超えている場合には、誤差共分散を算術右シフトし、カルマンフィルターの演算処理中にオーバーフローが発生しないように固定小数点位置を調整すること等が可能になる。
【００１７】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第二の閾値以下である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、前記左シフト量を前記演算部に通知し、前記演算部は、前記制御部から前記左シフト量を通知された場合に、前記左シフト量に基づき、前記誤差共分散を算術左シフトしてもよい。
【００１８】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第二の閾値以下である場合には、誤差共分散を算術左シフトし、カルマンフィルターの演算処理の正確度を高めるように固定小数点位置を調整すること等が可能になる。
【００１９】
また、本発明の一態様では、前記制御部は、過去の前記誤差共分散の正負判断を行い、過去の前記誤差共分散の正負に応じて、過去の前記誤差共分散に任意のビット列を加算し、前記ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行ってもよい。
【００２０】
これにより、誤差共分散の絶対値を求めずに、誤差共分散の正負の判断を行い、任意のビット列を加算して、その際のオーバーフローフラグの値に基づいて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【００２１】
また、本発明の一態様では、前記演算部は、飽和演算命令により、前記誤差共分散の前記演算処理を行い、前記制御部は、前記演算処理において、飽和が発生したと判断する場合には、前記誤差共分散の前記固定小数点位置を変更するためのシフト量を決定し、前記シフト量を演算部に通知し、前記演算部は、前記制御部から通知された前記シフト量に基づいて、前記演算処理前の前記誤差共分散を算術シフトし、前記固定小数点位置を変更してもよい。
【００２２】
これにより、カルマンフィルターの演算処理を飽和演算により行い、演算処理中に飽和が発生したか否かによって、固定小数点位置を変更する否かを決定し、固定小数点位置を調整すること等が可能になる。
【００２３】
また、本発明の一態様では、前記演算部は、前記誤差共分散が行列で表される場合に、前記演算処理の中で、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、前記制御部は、前記誤差共分散が行列で表される場合に、過去の前記誤差共分散行列の各要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うってもよい。
【００２４】
これにより、誤差共分散が行列で表される場合に、誤差共分散行列の各要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【００２５】
また、本発明の一態様では、前記制御部は、前記誤差共分散が行列で表される場合に、過去の前記誤差共分散行列の対角要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行ってもよい。
【００２６】
これにより、誤差共分散が行列で表される場合に、誤差共分散行列の対角要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【００２７】
また、本発明の一態様では、前記演算部は、前記誤差共分散及び状態推定値の予測処理と観測更新を行い、前記制御部は、前記予測処理における前記誤差共分散の前記固定小数点位置の前記決定処理を行い、前記観測更新における前記誤差共分散の前記固定小数点位置の前記決定処理を行ってもよい。
【００２８】
これにより、観測更新と予測処理のそれぞれについて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【００２９】
また、本発明の一態様では、前記制御部は、Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、前記誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行ってもよい。
【００３０】
これにより、Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＦＰＦＴ及びＱの固定小数点位置の調整すること等が可能になる。
【００３１】
また、本発明の一態様では、前記制御部は、Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行ってもよい。
【００３２】
これにより、Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＨＰＨＴ及びＲの固定小数点位置の調整すること等が可能になる。
【００３３】
また、本発明の他の態様では、前記状態推定装置を含むことを特徴とする電子機器に関係する。
【００３４】
また、本発明の他の態様では、上記各部としてコンピューターを機能させるプログラム、又は該プログラムを記憶したコンピューター読み取り可能な情報記憶媒体に関係する。
【図面の簡単な説明】
【００３５】
【図１】本実施形態の状態推定装置の構成例。
【図２】固定小数点と上位ビットの説明図。
【図３】図３（Ａ）、図３（Ｂ）は、算術右シフトにより固定小数点位置を変更する手法の説明図。
【図４】図４（Ａ）、図４（Ｂ）は、算術左シフトにより固定小数点位置を変更する手法の説明図。
【図５】図５（Ａ）〜図５（Ｄ）は、Ｐが正の値である場合に、第１のビット列を用いて固定小数点位置を変更する手法の説明図。
【図６】図６（Ａ）〜図６（Ｄ）は、Ｐが正の値である場合に、第２のビット列を用いて固定小数点位置を変更する手法の説明図。
【図７】図７（Ａ）〜図７（Ｄ）は、Ｐが負の値である場合に、第３のビット列を用いて固定小数点位置を変更する手法の説明図。
【図８】図８（Ａ）〜図８（Ｄ）は、Ｐが負の値である場合に、第４のビット列を用いて固定小数点位置を変更する手法の説明図。
【図９】固定小数点位置を調整しながらカルマンフィルターの演算を行う手法のフローチャート。
【図１０】固定小数点位置を調整する手法のフローチャート。
【発明を実施するための形態】
【００３６】
以下、本実施形態について説明する。なお、以下に説明する本実施形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また本実施形態で説明される構成の全てが、本発明の必須構成要件であるとは限らない。
【００３７】
１．概要
本実施形態では、カルマンフィルターの演算において、観測量や内部状態変数などを固定小数点数として扱い、演算を行う。これにより、内部状態変数等を浮動小数点数として演算を行う場合と比較して、計算量の削減を図ることができる。
【００３８】
また、本実施形態では、カルマンフィルターの演算において、オーバーフローが発生しないように、誤差共分散の固定小数点位置を動的に変更する。誤差共分散は、カルマンフィルターの演算の中で、最もダイナミックレンジが広いと考えられるためである。これにより、誤差共分散の演算中にオーバーフローが発生し、演算結果が不定値となり、演算が破綻することを防ぐことができる。
【００３９】
さらに、本実施形態では、誤差共分散以外の内部状態変数の一部又は全てについて、固定小数点位置を誤差共分散の固定小数点位置と一致させる。これにより、状態推定値や観測残差等の内部状態変数の固定小数点位置を、計算量を増加させずに、オーバーフローが発生しないように調整することができる。
【００４０】
以下、本実施形態のシステム構成例を説明し、その後、フローチャートを用いて本実施形態の処理の詳細について説明する。
【００４１】
２．システム構成例
次に、図１に本実施形態の状態推定装置及びこれを含む電子機器の構成例を示す。
【００４２】
状態推定装置１００は、演算部１２０と、制御部１４０と、記憶部１６０と、を含む。また、状態推定装置１００を含む電子機器の例としては、センサーや外部制御部や操作部や表示部などを含む携帯端末などが挙げられる。なお、状態推定装置１００及びこれを含む電子機器は、図１の構成に限定されず、これらの一部の構成要素を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。
【００４３】
次に各部で行われる処理について説明する。
【００４４】
演算部１２０は、カルマンフィルターの演算処理を行う。制御部１４０は、演算部１２０を制御する。記憶部１６０は、カルマンフィルターの演算処理の演算結果を記憶する。
【００４５】
ここで、カルマンフィルターの概要を簡単に説明する。
【００４６】
カルマンフィルターは、直接観測不可能な値を、観測可能な値を用いて精度良く推定する際に用いられる状態推定器である。
【００４７】
例えば、ある時刻ｔｋにおいて、あるシステムの観測不可能な状態量Ｘｋを求めたい場合を考える。Ｘｋは直接観測できない値であるが、逆にＸｋを用いれば、観測可能な値である観測量Ｚｋを、（１）式で表せることが分かっている。
【００４８】
【数１】
【００４９】
（１）式において、ｋはステップ数、Ｘｋは時刻ｔｋにおけるシステムの状態推定値、Ｚｋは時刻ｔｋにおいて入力される観測量、Ｈｋは観測行列、Ｖｋは観測雑音を示している。なお、Ｈｋは物理法則や測定等によって予め確定した行列である。（１）式を観測方程式という。
【００５０】
また、時刻ｔｋにおけるシステムの状態量Ｘｋは、１ステップ前の時刻ｔｋ-１の状態量Ｘｋ-１をもとに、次のように表現されることが分かっているとする。
【００５１】
【数２】
【００５２】
（２）式において、Ｘｋ-１は時刻ｔｋ-１におけるシステムの状態量、Ｆｋは状態遷移行列、Ｂｋは駆動行列、Ｕｋは制御入力、Ｗｋはシステム雑音（白色雑音）を示している。なお、Ｆｋ及びＢｋは、物理法則や測定等によって予め確定した行列である。（２）式を状態方程式という。
【００５３】
ここで、（１）式に観測により得られた観測量Ｚｋを代入すれば、真の状態量Ｘｋが求められるようにも思えるが、観測雑音Ｖｋが不明であるため、（１）式から直接に真の状態量Ｘｋを求めることはできない。
【００５４】
そこで、まず、（２）式において、システム雑音Ｗｋがないと仮定した場合を考え、その場合の（２）式に相当する（３）式により、状態推定値Ｘｋ|ｋ-１を求める。
【００５５】
【数３】
【００５６】
（３）式において、Ｘｋ|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値であり、Ｘｋ-１|ｋ-１は時刻ｔｋ-１において推定した時刻ｔｋ-１のシステムの状態推定値である。
【００５７】
次に、（１）式において、観測誤差がないと仮定した場合を考え、その場合の（１）式に相当する（４）式により、観測誤差がないと仮定した場合の観測量の予測値Ｚｋ，eを求める。（４）式のＸｋ|ｋ-１は（３）式により求められたものである。
【００５８】
【数４】
【００５９】
そして、実際に観測により得られた観測量Ｚｋ，ｒとＺｋ，eとの差をとる。
【００６０】
【数５】
【００６１】
Δｋを観測残差という。観測残差は、以下の３値に起因する値である。一つ目の値は、雑音がないとした場合における、状態推定値から求めた観測量の推定値（観測推定量）と、実際の観測量との誤差である。そして、二つ目の値は、観測雑音Ｖｋであり、三つ目の値は、システム雑音Ｗｋである。
【００６２】
さらに、観測残差Δｋの共分散Ｓｋを算出する。観測残差Δｋの共分散Ｓｋは、システムの状態推定値Ｘｋから求められた観測推定量の確からしさと、観測雑音とシステム雑音がどの程度含まれているかを表すものである。つまり、観測残差Δｋの共分散Ｓｋをできるだけ小さくすることができれば、観測雑音とシステム雑音を抑圧しつつ、観測推定量を確からしいものにすることができる。さらに、観測推定量の正確性が担保されれば、観測推定量を求める際に参照された状態推定値の正確さも期待できる。
【００６３】
したがって、観測残差Δｋの共分散Ｓｋをできるだけ小さくするように制御すれば、システムの状態推定値も確からしいものとなる。
【００６４】
カルマンフィルターでは、以上の原理を基に、観測更新と予測処理との２つの処理を繰り返し行う。観測更新とは、外部から与えられる（実際に観測した）観測量Ｚｋ，ｒを利用して１ステップ前に予測した状態推定値Ｘｋ｜ｋ-１及び誤差共分散Ｐｋ｜ｋ-１を更新する処理である。一方、予測処理とは、更新した状態推定値Ｘｋ｜ｋ及び誤差共分散Ｐｋ｜ｋから次のステップの状態推定値Ｘｋ+１｜ｋ及び誤差共分散Ｐｋ+１｜ｋを予測する処理である。そして、カルマンフィルターは、この２つの処理を繰り返すことにより、尤もらしい状態推定値Ｘｋを推定する。
【００６５】
まず、観測更新は、次式（６）〜（９）で定式化される。
【００６６】
【数６】
【００６７】
【数７】
【００６８】
【数８】
【００６９】
【数９】
【００７０】
なお、（６）〜（９）において、Ｓｋは、上述した観測残差Δｋの共分散である。Ｐｋ｜ｋ-１は、時刻ｔｋ-１において推定した時刻ｔｋのシステムの状態推定値Ｘｋ｜ｋ−１と、時刻ｔｋの実際の状態量Ｘｋとの差の共分散（誤差共分散）である。また、観測行列Ｈｋは、状態推定値Ｘｋの次元を観測量Ｚｋの次元に変換するための変換行列として用いられる。観測行列Ｈｋを１ステップ前に予測された状態推定値Ｘｋ｜ｋ-１に作用させることで、観測量の予測値Ｚｋ，eが得られる。さらに、Ｋｋはカルマンゲインと呼ばれる補正係数である。カルマンゲインＫｋは、予測された状態推定値Ｘｋ｜ｋ-１を重視するか、Ｚｋ，ｒとＺｋ，eとの差Δｋを重視するかを決定付けるパラメーター値として用いられる。カルマンゲインＫｋは、観測誤差Ｒｋの大きさによって決定付けられる。観測誤差Ｒｋは、観測量Ｚｋに含まれる想定誤差の大きさを表すパラメーター（想定誤差パラメーター）である。観測誤差Ｒｋは、期待値（平均値）を“０”とする正規分布型のホワイトノイズと仮定して設定される。なお、Ｉは単位行列である。
【００７１】
式（８）は、Ｚｋ，ｒとＺｋ，eとの差Δｋを用いて、状態推定値Ｘｋ｜ｋを更新することを表している。
【００７２】
また、式（９）は、１ステップ前に予測された誤差共分散Ｐｋ｜ｋ-１とカルマンゲインＫｋと観測行列Ｈｋとを用いて、誤差共分散Ｐｋ｜ｋを更新することを表している。
【００７３】
一方、予測処理は、次式（１０）及び（１１）で定式化される。
【００７４】
【数１０】
【００７５】
【数１１】
【００７６】
なお、（１１）式において、Ｑｋはシステム雑音と呼ばれ、システムのモデル誤差を表す正規分布に従う誤差行列として定義される。状態遷移行列Ｆｋは、観測更新で更新された状態推定値Ｘｋ|ｋから次のステップの状態推定値Ｘｋ+１|ｋへの遷移を表す行列として定義される。
【００７７】
（３）式〜（１１）式にて、表される値等をまとめて、カルマンフィルターの内部状態変数と呼ぶ。
【００７８】
以上がカルマンフィルターの概要である。なお、カルマンフィルターは拡張カルマンフィルターであってもよい。
【００７９】
以上の本実施形態では、カルマンフィルターの演算処理を行う演算部１２０と、演算部１２０を制御する制御部１４０と、カルマンフィルターの演算処理の演算結果を記憶する記憶部１６０と、を含む。そして、演算部１２０は、演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、制御部１４０は、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。
【００８０】
固定小数点数とは、コンピューターにおける実数の近似値の表現方式の一つである。固定小数点数では整数部分に用いるビット数と小数部分に用いるビット数をあらかじめ固定して表現する。具体的な例を図２に示す。図２では、３２ビットの固定小数点数により、誤差共分散Ｐを表現しており、ＦＰが固定小数点である。
【００８１】
また、固定小数点位置とは、固定小数点の位置のことをいい、図２においては、ビット番号２５と２６の間が固定小数点位置となる。なお、固定小数点位置はビット番号０〜３１の範囲内である必要はなく、例えばビット番号１６〜４７の範囲内で調整するものであってもよい。
【００８２】
本実施形態の一態様では、過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置の決定処理を行う。よって、誤差共分散の固定小数点位置を調整しつつ、カルマンフィルターの演算処理を行うことが可能となる。
【００８３】
これにより、浮動小数点数によりカルマンフィルターの演算処理を行う場合に対して、少ない計算量で、演算処理を行うことができる。また、オーバーフローを発生させないように、固定小数点位置を調整して、演算することができる。
【００８４】
また、制御部１４０は、誤差共分散についての過去の演算結果に基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。
【００８５】
これにより、過去の誤差共分散の演算結果に基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００８６】
また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。
【００８７】
上位ビットとは、誤差共分散を固定小数点数により表した場合のＭＳＢ（Most Significant Bit）側に位置する任意の数のビットのことである。例えば、図２においては、上位ビット数を４とし、ＨＢを上位ビットとしている。
【００８８】
これにより、過去の誤差共分散の絶対値における上位ビットに基づいて、現在の誤差共分散の固定小数点位置を調整し、カルマンフィルターの演算処理を行うこと等が可能になる。
【００８９】
また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットにより表される値が、第一の閾値以上である場合には、決定処理として、誤差共分散の固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、右シフト量を演算部１２０に通知し、演算部１２０は、制御部１４０から右シフト量を通知された場合に、右シフト量に基づき、誤差共分散を算術右シフトしてもよい。
【００９０】
上位ビットにより表される値とは、具体的に図２においては、２進数で０００１、１０進数では１となる。
【００９１】
また、第一の閾値とは、算術右シフトを行うか否かを判断するための閾値である。
【００９２】
さらに、シフト量とは、算術シフトを行う際のビット移動量のことをいう。また、ＬＳＢ（Least Significant Bit）側に算術シフトすることを算術右シフトといい、ＭＳＢ側に算術シフトすることを算術左シフトという。シフト量の中で、算術右シフトをする際のシフト量のことを右シフト量といい、算術左シフトをする際のシフト量のことを左シフト量という。
【００９３】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第一の閾値を超えている場合には、誤差共分散を算術右シフトし、カルマンフィルターの演算処理中にオーバーフローが発生しないように固定小数点位置を調整すること等が可能になる。
【００９４】
具体例を図３（Ａ）、図３（Ｂ）に示す。図３（Ａ）においては、誤差共分散Ｐを浮動小数点数により表している。図３（Ａ）においては、固定小数点がＦＰ１であり、Ｐの整数部分がＩＰ１、小数部分がＤＰ１に相当する。さらに、第一の閾値を１０進数で３としている。この場合においては、上位ビットにより表される値が７であり、第一の閾値以上であるため、算術右シフトを行うことが決定される。なお、この例では、右シフト量を一律１と設定している。
【００９５】
一方、図３（Ｂ）には、算術右シフト後の誤差共分散Ｐを表している。算術右シフト後には、Ｐの整数部分がＩＰ２、小数部分がＤＰ２となる。その結果、整数部分がＩＰ１からＩＰ２へと拡大され、オーバーフローが発生しにくくなる。
【００９６】
また、制御部１４０は、過去の誤差共分散の絶対値における上位ビットにより表される値が、第二の閾値以下である場合には、決定処理として、誤差共分散の固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、左シフト量を演算部に通知し、演算部１２０は、制御部１４０から左シフト量を通知された場合に、左シフト量に基づき、誤差共分散を算術左シフトしてもよい。
【００９７】
第二の閾値とは、算術左シフトを行うか否かを判断するための閾値である。
【００９８】
これにより、過去の誤差共分散の絶対値における上位ビットにより表される値が第二の閾値以下である場合には、誤差共分散を算術左シフトし、カルマンフィルターの演算処理の正確度を高めるように固定小数点位置を調整すること等が可能になる。
【００９９】
具体例を図３（Ａ）、図３（Ｂ）と同様に、図４（Ａ）、図４（Ｂ）に示す。図４（Ａ）においては、固定小数点がＦＰ３であり、Ｐの整数部分がＩＰ３、小数部分がＤＰ３に相当する。さらに、第二の閾値を１０進数で０としている。この場合においては、上位ビットにより表される値が０であり、第二の閾値以下であるため、算術左シフトを行うことが決定される。なお、この例では、左シフト量を一律１と設定している。
【０１００】
一方、図４（Ｂ）には、算術左シフト後の誤差共分散Ｐを表している。算術左シフト後には、Ｐの整数部分がＩＰ４、小数部分がＤＰ４となる。その結果、小数部分がＤＰ３からＤＰ４へと拡大され、より正確度の高い演算処理を行うことができるようになる。
【０１０１】
また、制御部１４０は、過去の誤差共分散の正負判断を行い、過去の誤差共分散の正負に応じて、過去の誤差共分散に任意のビット列を加算し、ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の誤差共分散の固定小数点位置についての決定処理を行ってもよい。
【０１０２】
任意のビット列とは、誤差共分散について演算処理を行う場合に、オーバーフローが発生するか否かを擬似的に判定するために用いるビット列のことをいう。具体例については後述する。
【０１０３】
また、オーバーフローフラグとは、ステータスレジスターの中のフラグビットの一つであり、カルマンフィルターの演算において、オーバーフローが発生したか否かをビットの状態によって判別することができるフラグのことをいう。なお、オーバーフローフラグをゼロフラグなどの他のフラグビットに置き換えて、同様の機能を実現する実施形態も本発明に含まれるのものとする。
【０１０４】
具体例を図５（Ａ）〜図８（Ｄ）を用いて説明する。
【０１０５】
図５（Ａ）〜図６（Ｄ）は、誤差共分散Ｐの値が正である場合を示している。
【０１０６】
図５（Ａ）は、固定小数点がＦＰ５である正の誤差共分散Ｐを示しており、図５（Ｂ）は任意のビット列Ｍを示している。このビット列Ｍは、誤差共分散が正の場合において、誤差共分散を算術右シフトして、整数部分を拡大させるか否かの判断をするために用いられる。本例では、誤差共分散Ｐの上位４ビットにより表される値が１０進数で３以上であれば、オーバーフローが発生する可能性が高いと仮定し、その際には算術右シフトを行い、誤差共分散Ｐの整数部分を拡大させる。そこで、この判断を行う際に、図５（Ｂ）に示すビット列Ｍを用い、整数部分を拡大する必要があるかを判断する。この判断は、図５（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算したビット列Ｐ＆Ｍを求め、この時、オーバーフローフラグＯＦが１であるか否かを判断することによって行われる。この例においては、オーバーフローフラグが１であるので、上位４ビットにより表される値が３以上であると判断し、右シフト量を１とする算術右シフトを行う。その様子を示しているのが図５（Ｄ）であり、固定小数点がＦＰ５からＦＰ６に移動している。これにより、整数部分が拡大し、オーバーフローが発生しにくくなっている。一方、上位４ビットにより表される値が３よりも小さい場合には、何もしない。
【０１０７】
次に、図６（Ａ）は、固定小数点がＦＰ７である正の誤差共分散Ｐを示しており、図６（Ｂ）は任意のビット列Ｍを示している。このビット列Ｍは誤差共分散が正の場合において、誤差共分散を算術左シフトして、小数部分を拡大させるか否かの判断をするために用いられる。つまり、誤差共分散Ｐが正である場合、負である場合のそれぞれにおいて、算術右シフトをするか否かを判断するビット列と、算術左シフトをするか否かを判断するビット列の２種類のビット列をあらかじめ設定しておく。本例では、誤差共分散Ｐの上位４ビットにより表される値が１０進数で０であれば、演算処理の正確度が低下する可能性が高いと仮定し、その際には算術左シフトを行い、誤差共分散Ｐの小数部分を拡大させる。そこで、この判断を行う際に、図６（Ｂ）に示すビット列Ｍを用い、小数部分を拡大する必要があるかを判断する。この判断は、図６（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算したビット列Ｐ＆Ｍを求め、この時、オーバーフローフラグＯＦが０であるか否かを判断することによって行われる。この例においては、オーバーフローフラグが０であるので、上位４ビットにより表される値が０であると判断し、左シフト量を１とする算術左シフトを行う。その様子を示しているのが図６（Ｄ）であり、固定小数点がＦＰ７からＦＰ８に移動している。これにより、小数部分が拡大し、演算処理の正確度の向上を期待することができる。一方、上位４ビットにより表される値が０よりも大きい場合には、何もしない。
【０１０８】
図７（Ａ）〜図８（Ｄ）は、誤差共分散Ｐの値が負である場合を示している。
【０１０９】
誤差共分散Ｐの値が負である場合にも、同様に２種類のビット列を用意しておく。その一例として、図７（Ｂ）に示す任意のビット列Ｍは、Ｐの上位４ビットにより表される値が−３以下であるかを判断するために用いられる。一方、図８（Ｂ）に示す任意のビット列Ｍは、Ｐの上位４ビットにより表される値が−１であるかを判断するために用いられる。
【０１１０】
例えば、図７（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算した結果、オーバーフローフラグが０であるので、上位４ビットにより表される値が−３以下であると判断し、右シフト量を１とする算術右シフトを行う。その様子を示しているのが図７（Ｄ）であり、固定小数点がＦＰ９からＦＰ１０に移動している。これにより整数部分が拡大される。一方、上位４ビットにより表される値が−３よりも小さい場合、すなわちオーバーフローフラグが１である場合には、何もしない。
【０１１１】
同様に、図８（Ｃ）に示すように、誤差共分散Ｐに任意のビット列Ｍを加算した結果、オーバーフローフラグが１であるので、上位４ビットにより表される値が−１であると判断し、左シフト量を１とする算術左シフトを行う。その様子を示しているのが図８（Ｄ）であり、固定小数点がＦＰ１１からＦＰ１２に移動している。これにより小数部分が拡大される。一方、上位４ビットにより表される値が−１よりも小さい場合、すなわちオーバーフローフラグが０である場合には、何もしない。
【０１１２】
このように、誤差共分散の絶対値を求めずに、誤差共分散の正負の判断を行い、任意のビット列を加算して、その際のオーバーフローフラグの値に基づいて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【０１１３】
また、演算部１２０は、飽和演算命令により、誤差共分散の演算処理を行い、制御部１４０は、演算処理において、飽和が発生したと判断する場合には、誤差共分散の固定小数点位置を変更するためのシフト量を決定し、シフト量を演算部１２０に通知し、演算部１２０は、制御部１４０から通知されたシフト量に基づいて、演算処理前の誤差共分散を算術シフトし、固定小数点位置を変更してもよい。
【０１１４】
飽和演算とは、サチュレート演算ともいい、数値の上限と下限が決まっており、演算結果がその値を超えた場合は上限値又は下限値になる演算をいう。
【０１１５】
これにより、カルマンフィルターの演算処理を飽和演算により行い、演算処理中に飽和が発生したか否かによって、固定小数点位置を変更する否かを決定し、固定小数点位置を調整すること等が可能になる。
【０１１６】
また、演算部１２０は、誤差共分散が行列で表される場合に、演算処理の中で、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、制御部１４０は、誤差共分散が行列で表される場合に、過去の誤差共分散行列の各要素に基づいて、現在の誤差共分散行列の各要素についての固定小数点位置の決定処理を行ってもよい。
【０１１７】
行列の各要素の固定小数点位置が異なると、積和の度に固定小数点位置の整合性を取る必要があるため、一つの行列内では同じ固定小数点位置で表現することとする。
【０１１８】
具体的には、誤差共分散が行列で表される場合というのは、観測量Ｚが行列である場合である。この場合には、（６）式、（９）式、（１１）式における誤差共分散Ｐも行列で表される。なお、それに伴い（４）式から（１１）式における各項（例えば、Ｘ、Ｈ、Ｓなど）も行列で表される。
【０１１９】
ここで、状態推定値Ｘについての誤差共分散は、Ｘを計算した時の確からしさを意味する。
【０１２０】
また、状態推定値Ｘが（１２）式で表すような行列であり、
【０１２１】
【数１２】
【０１２２】
Ｘについての誤差共分散Ｐが（１３）式で表すような行列である場合に、
【０１２３】
【数１３】
【０１２４】
誤差共分散の要素ｐｉｊは、ｘｉの値を計算した時のｘｊの値の確からしさを表す。
【０１２５】
本実施形態によれば、例えば、誤差共分散の要素の絶対値の中で最も大きい値ｐｋｌの値に基づき、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【０１２６】
また、制御部１４０は、誤差共分散が行列で表される場合に、過去の誤差共分散行列の対角要素に基づいて、現在の誤差共分散行列の各要素についての固定小数点位置の決定処理を行ってもよい。
【０１２７】
対角要素は、行列の対格線上にある要素のことをいい、（１３）式の例でいうと、ｐｉｉ等ことである。ｐｉｉは、ｘｉの値を計算した時のｘｉ自身の確からしさ、いわゆる自己相関を表す。そのため、ｘｉの値の確からしさを知りたい場合には最も有用な情報となる。
【０１２８】
これにより、誤差共分散が行列で表される場合に、誤差共分散行列の対角要素の値に基づいて、誤差共分散行列の各要素についての固定小数点位置を調整すること等が可能になる。
【０１２９】
また、演算部１２０は、誤差共分散及び状態推定値の予測処理と観測更新を行い、制御部１４０は、予測処理における誤差共分散の固定小数点位置の決定処理を行い、観測更新における誤差共分散の固定小数点位置の決定処理を行ってもよい。
【０１３０】
これにより、観測更新と予測処理のそれぞれについて、誤差共分散の固定小数点位置を調整すること等が可能になる。
【０１３１】
また、制御部１４０は、（１１）式に示すＰ＝ＦＰＦＴ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行ってもよい。
【０１３２】
内部状態変数の加算時において、固定小数点位置の余計な調整を行わなずに、そのまま加算を行うためには、加算する内部状態変数の固定小数点位置をあらかじめ揃えておく必要がある。
【０１３３】
例えば、（１１）式のＰ＝ＦＰＦＴ＋Ｑの演算を考える。この場合、ＦＰＦＴとＱの固定小数点位置が、左辺のＰと同じ固定小数点位置であれば、固定小数点位置の調整をせずに加算を行うことができる。つまり、Ｐと、ＦＰＦＴ、Ｑの固定小数点位置を全て同一にすればよい。
【０１３４】
これにより、Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＦＰＦＴ及びＱの固定小数点位置の調整すること等が可能になる。
【０１３５】
また、制御部１４０は、（６）式に示すＳ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行ってもよい。
【０１３６】
例えば、（６）式のＳ＝ＨＰＨＴ＋Ｒの演算を考える。この場合、ＨＰＨＴとＲの固定小数点位置が、左辺のＳと同じ固定小数点位置であれば、固定小数点位置の調整をせずに加算を行うことができる。つまり、Ｓと、ＨＰＨＴ、Ｒの固定小数点位置を全て同一にすればよい。さらに、ＨＰＨＴの固定小数点位置を、Ｐの固定小数点位置と同一にすれば、Ｐと、ＦＰＦＴ、ＨＰＨＴ、Ｑ、Ｓ、Ｒの固定小数点位置を全て同一にすることができ、演算量のさらなる削減を図ることができる。
【０１３７】
これにより、Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの固定小数点位置を、誤差共分散Ｐの固定小数点位置と同じ位置とする決定処理を行い、計算量を増加させずに、ＨＰＨＴ及びＲの固定小数点位置の調整すること等が可能になる。
【０１３８】
なお本実施形態の状態推定装置等は、プログラムにより実現してもよい。この場合には、ＣＰＵ等のプロセッサーがプログラムを実行することで、本実施形態の状態推定装置等が実現される。具体的には情報記憶媒体に記憶されたプログラムが読み出され、読み出されたプログラムをＣＰＵ等のプロセッサーが実行する。ここで、情報記憶媒体（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク（ＤＶＤ、ＣＤ等）、ＨＤＤ（ハードディスクドライブ）、或いはメモリー（カード型メモリー、ＲＯＭ等）などにより実現できる。そしてＣＰＵ等のプロセッサーは、情報記憶媒体に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。
【０１３９】
３．処理の流れ
以下では、図９と図１０のフローチャートを用いて、本実施形態の処理の流れについて説明する。
【０１４０】
３．１ 固定小数点位置を調整しながらカルマンフィルターの演算を行う処理の詳細
図９のフローチャートに示すように、まず、一定期間が経過しているかを判断する（Ｓ１）。一定期間が経過している場合には、固定小数点位置の調整を行う（Ｓ２）。固定小数点位置の調整後、または一定期間が経過していないと判断する場合には、カルマンフィルターの観測更新処理を行う（Ｓ３）。そして、最後に予測処理を行う（Ｓ４）。
【０１４１】
以上の処理を一定周期で行う。
【０１４２】
３．１．１ 固定小数点位置を調整する処理の詳細
図１０のフローチャートに示すように、前回の誤差共分散行列の各要素の絶対値のうち最大となる値を特定する（Ｓ１０）。次に、特定した値に上位４ビットにより表される値を特定する（Ｓ１１）。そして、この上位４ビットにより表される値が第一の閾値より大きいかを判断する（Ｓ１２）。第一の閾値以上であると判断する場合には、右シフト量を決定する（Ｓ１３）。一方、第一の閾値以下であると判断する場合には、上位４ビットにより表される値が第二の閾値以下であるかを判断する（Ｓ１４）。第二の閾値以下であると判断する場合には、左シフト量を決定する（Ｓ１５）。右シフト量又は左シフト量が決定された場合には、シフト量に基づいて誤差共分散Ｐを算術シフトする（Ｓ１６）。最後に、算術シフト後、または、上位ビットにより表される値が第二の閾値以下であると判断する場合には、ＦＰＦＴ、ＨＰＨＴ、Ｑ、Ｓ、Ｒの固定小数点位置を、全てＰの固定小数点位置と同一にする（Ｓ１７）。
【０１４３】
以上のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また状態推定装置及びこれを含む電子機器の構成、動作も本実施形態で説明したものに限定されず、種々の変形実施が可能である。
【符号の説明】
【０１４４】
１００ 状態推定装置、１２０ 演算部、１４０ 制御部、１６０ 記憶部
【特許請求の範囲】
【請求項１】
カルマンフィルターの演算処理を行う演算部と、
前記演算部を制御する制御部と、
前記カルマンフィルターの演算処理の演算結果を記憶する記憶部と、
を含み、
前記演算部は、
前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、
前記制御部は、
過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とする状態推定装置。
【請求項２】
請求項１において、
前記制御部は、
前記誤差共分散についての過去の前記演算結果に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
【請求項３】
請求項２において、
前記制御部は、
過去の前記誤差共分散の絶対値における上位ビットに基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
【請求項４】
請求項３において、
前記制御部は、
過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第一の閾値以上である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、前記右シフト量を前記演算部に通知し、
前記演算部は、
前記制御部から通知された前記右シフト量に基づき、前記誤差共分散を算術右シフトすることを特徴とする状態推定装置。
【請求項５】
請求項３又は４において、
前記制御部は、
過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第二の閾値以下である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、前記左シフト量を前記演算部に通知し、
前記演算部は、
前記制御部から通知された前記左シフト量に基づき、前記誤差共分散を算術左シフトすることを特徴とする状態推定装置。
【請求項６】
請求項２において、
前記制御部は、
過去の前記誤差共分散の正負判断を行い、過去の前記誤差共分散の正負に応じて、過去の前記誤差共分散に任意のビット列を加算し、前記ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
【請求項７】
請求項２において、
前記演算部は、
飽和演算命令により、前記誤差共分散の前記演算処理を行い、
前記制御部は、
前記演算処理において、飽和が発生したと判断する場合には、前記誤差共分散の前記固定小数点位置を変更するためのシフト量を決定し、前記シフト量を演算部に通知し、
前記演算部は、
前記制御部から通知された前記シフト量に基づいて、前記演算処理前の前記誤差共分散を算術シフトし、前記固定小数点位置を変更することを特徴とする状態推定装置。
【請求項８】
請求項２乃至７のいずれかにおいて、
前記演算部は、
前記誤差共分散が行列で表される場合に、前記演算処理として、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、
前記制御部は、
過去の前記誤差共分散行列の各要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
【請求項９】
請求項８において、
前記制御部は、
過去の前記誤差共分散行列の対角要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
【請求項１０】
請求項１乃至９のいずれかにおいて、
前記演算部は、
前記誤差共分散及び状態推定値の予測処理と観測更新を行い、
前記制御部は、
前記予測処理における前記誤差共分散の前記固定小数点位置の前記決定処理を行い、前記観測更新における前記誤差共分散の前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
【請求項１１】
請求項１０において、
前記制御部は、
Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、前記誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行うことを特徴とする状態推定装置。
【請求項１２】
請求項１０又は１１において、
前記制御部は、
Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行うことを特徴とする状態推定装置。
【請求項１３】
請求項１乃至１２のいずれかに記載の状態推定装置を含むことを特徴とする電子機器。
【請求項１４】
カルマンフィルターの演算処理を行う演算部と、
前記演算部を制御する制御部と、
前記カルマンフィルターの演算処理の演算結果を記憶する記憶部として、
コンピューターを機能させ、
前記演算部は、
前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、
前記制御部は、
過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とするプログラム。
【請求項１】
カルマンフィルターの演算処理を行う演算部と、
前記演算部を制御する制御部と、
前記カルマンフィルターの演算処理の演算結果を記憶する記憶部と、
を含み、
前記演算部は、
前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、
前記制御部は、
過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とする状態推定装置。
【請求項２】
請求項１において、
前記制御部は、
前記誤差共分散についての過去の前記演算結果に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
【請求項３】
請求項２において、
前記制御部は、
過去の前記誤差共分散の絶対値における上位ビットに基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
【請求項４】
請求項３において、
前記制御部は、
過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第一の閾値以上である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術右シフトにより変更するために用いられるシフト量である右シフト量を決定する処理を行い、前記右シフト量を前記演算部に通知し、
前記演算部は、
前記制御部から通知された前記右シフト量に基づき、前記誤差共分散を算術右シフトすることを特徴とする状態推定装置。
【請求項５】
請求項３又は４において、
前記制御部は、
過去の前記誤差共分散の絶対値における前記上位ビットにより表される値が、第二の閾値以下である場合には、前記決定処理として、前記誤差共分散の前記固定小数点位置を算術左シフトにより変更するために用いられるシフト量である左シフト量を決定する処理を行い、前記左シフト量を前記演算部に通知し、
前記演算部は、
前記制御部から通知された前記左シフト量に基づき、前記誤差共分散を算術左シフトすることを特徴とする状態推定装置。
【請求項６】
請求項２において、
前記制御部は、
過去の前記誤差共分散の正負判断を行い、過去の前記誤差共分散の正負に応じて、過去の前記誤差共分散に任意のビット列を加算し、前記ビット列の加算によって得られるオーバーフローフラグの値に基づいて、現在の前記誤差共分散の前記固定小数点位置についての前記決定処理を行うことを特徴とする状態推定装置。
【請求項７】
請求項２において、
前記演算部は、
飽和演算命令により、前記誤差共分散の前記演算処理を行い、
前記制御部は、
前記演算処理において、飽和が発生したと判断する場合には、前記誤差共分散の前記固定小数点位置を変更するためのシフト量を決定し、前記シフト量を演算部に通知し、
前記演算部は、
前記制御部から通知された前記シフト量に基づいて、前記演算処理前の前記誤差共分散を算術シフトし、前記固定小数点位置を変更することを特徴とする状態推定装置。
【請求項８】
請求項２乃至７のいずれかにおいて、
前記演算部は、
前記誤差共分散が行列で表される場合に、前記演算処理として、固定小数点数として各要素を表現される誤差共分散行列の演算処理を行い、
前記制御部は、
過去の前記誤差共分散行列の各要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
【請求項９】
請求項８において、
前記制御部は、
過去の前記誤差共分散行列の対角要素に基づいて、現在の前記誤差共分散行列の各要素についての前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
【請求項１０】
請求項１乃至９のいずれかにおいて、
前記演算部は、
前記誤差共分散及び状態推定値の予測処理と観測更新を行い、
前記制御部は、
前記予測処理における前記誤差共分散の前記固定小数点位置の前記決定処理を行い、前記観測更新における前記誤差共分散の前記固定小数点位置の前記決定処理を行うことを特徴とする状態推定装置。
【請求項１１】
請求項１０において、
前記制御部は、
Ｐ＝ＦＰＦＴ＋Ｑの演算処理により、前記誤差共分散Ｐを求める場合に、ＦＰＦＴ及びＱの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行うことを特徴とする状態推定装置。
【請求項１２】
請求項１０又は１１において、
前記制御部は、
Ｓ＝ＨＰＨＴ＋Ｒの演算処理により、観測残差の共分散Ｓを求める場合に、ＨＰＨＴ及びＲの前記固定小数点位置を、前記誤差共分散Ｐの前記固定小数点位置と同じ位置とする前記決定処理を行うことを特徴とする状態推定装置。
【請求項１３】
請求項１乃至１２のいずれかに記載の状態推定装置を含むことを特徴とする電子機器。
【請求項１４】
カルマンフィルターの演算処理を行う演算部と、
前記演算部を制御する制御部と、
前記カルマンフィルターの演算処理の演算結果を記憶する記憶部として、
コンピューターを機能させ、
前記演算部は、
前記演算処理として、固定小数点数として表現される誤差共分散の演算処理を行い、
前記制御部は、
過去の前記演算結果に基づいて、現在の前記誤差共分散の固定小数点位置の決定処理を行うことを特徴とするプログラム。
【図１】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２０１２−２０３５６６（Ｐ２０１２−２０３５６６Ａ）
【公開日】平成２４年１０月２２日（２０１２．１０．２２）
【国際特許分類】
【出願番号】特願２０１１−６６３９８（Ｐ２０１１−６６３９８）
【出願日】平成２３年３月２４日（２０１１．３．２４）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【公開日】平成２４年１０月２２日（２０１２．１０．２２）
【国際特許分類】
【出願日】平成２３年３月２４日（２０１１．３．２４）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
[ Back to top ]