JP2012237704A - Terrestrial magnetism measuring device, terrestrial magnetism measuring method and terrestrial magnetism measurement program - Google Patents
Terrestrial magnetism measuring device, terrestrial magnetism measuring method and terrestrial magnetism measurement program Download PDFInfo
- Publication number
- JP2012237704A JP2012237704A JP2011108125A JP2011108125A JP2012237704A JP 2012237704 A JP2012237704 A JP 2012237704A JP 2011108125 A JP2011108125 A JP 2011108125A JP 2011108125 A JP2011108125 A JP 2011108125A JP 2012237704 A JP2012237704 A JP 2012237704A
- Authority
- JP
- Japan
- Prior art keywords
- value
- observation
- variable
- state
- geomagnetism
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
Images
Landscapes
- Navigation (AREA)
- Measuring Magnetic Variables (AREA)
Abstract
Description
本発明は、地磁気測定装置、地磁気測定方法および地磁気測定プログラムに関する。 The present invention relates to a geomagnetism measuring apparatus, a geomagnetism measuring method, and a geomagnetism measuring program.
地磁気の大きさ及び方向を算出する場合、地磁気センサの出力の他に、角速度センサ、加速度センサ等の異種のセンサ出力を統合することで、地磁気センサの出力にノイズが含まれる場合であっても、正確な値を高速に算出することが期待される。 When calculating the magnitude and direction of geomagnetism, it is possible to integrate different sensor outputs such as angular velocity sensors and acceleration sensors in addition to the output of the geomagnetic sensor, so that the output of the geomagnetic sensor includes noise. It is expected to calculate an accurate value at high speed.
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、拡張カルマンフィルタ(EKF)やシグマポイントカルマンフィルタ(SPKF)等の非線形カルマンフィルタが知られている。例えば、特許文献1には、3軸の角速度センサ及び3軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献1には、3軸角速度センサ、3軸加速度センサ、及び3軸地磁気センサからの出力信号を、拡張カルマンフィルタやアンセンテッド変換を用いたシグマポイントカルマンフィルタを用いて統合し、姿勢を推定する方法が開示されている。
Nonlinear Kalman filters such as an extended Kalman filter (EKF) and a sigma point Kalman filter (SPKF) are known as methods for integrating the outputs of a plurality of sensors that measure different physical quantities and estimating the state of a dynamic system. For example,
一般的に、非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、推定された動的システムの状態から、動的システムの有する複数のセンサが計測する値(観測値)を推定する観測モデルと、を有する。そして、非線形カルマンフィルタは、推定された観測値と、複数のセンサが実際に測定する観測値との差分(観測残差)を用いて、動的システムの状態を表す複数の物理量を要素とする状態ベクトルを、より真値に近い値へと更新する。
しかし、非線形カルマンフィルタを用いた演算を繰り返す場合、観測値等に重畳しているノイズの影響や、状態遷移モデルと観測モデルとが有する非線形性の影響により、動的システムの状態が実際の値から大きく外れた値として推定される場合が存在するという問題点がある。
In general, a nonlinear Kalman filter is composed of a state transition model that estimates changes over time in a plurality of physical quantities representing the state of a dynamic system, and a plurality of sensors that the dynamic system has based on the estimated state of the dynamic system. An observation model for estimating a value to be measured (observation value). The nonlinear Kalman filter uses a difference (observation residual) between the estimated observation value and the observation value actually measured by a plurality of sensors, and uses a plurality of physical quantities representing the state of the dynamic system as elements. Update the vector to a value closer to the true value.
However, when the calculation using a nonlinear Kalman filter is repeated, the state of the dynamic system is changed from the actual value due to the influence of noise superimposed on the observation value, etc. and the nonlinearity of the state transition model and the observation model. There is a problem that there is a case where the value is estimated as a greatly deviated value.
本発明は、上述した点に鑑みてなされたものであり、非線形カルマンフィルタを用いて動的システムの状態を推定する場合に、推定される値が実際の値から大きく外れた不適切な値になることを防止し、正確な地磁気の値を算出することを解決課題とする。 The present invention has been made in view of the above-described points, and when a state of a dynamic system is estimated using a nonlinear Kalman filter, an estimated value is an inappropriate value greatly deviating from an actual value. It is an object to solve this problem and to calculate an accurate value of geomagnetism.
上記課題を解決するため、本発明に係る地磁気測定装置は、システムを観測して観測値を各々出力する複数のセンサと、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行するカルマンフィルタと、を備え、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする。 In order to solve the above problems, a geomagnetism measuring apparatus according to the present invention includes a plurality of sensors for observing a system and outputting observation values, and a plurality of state variables indicating a state of the system using a state transition model. A state vector is estimated, an estimated observation value is calculated from the state vector using an observation value model, a difference between the estimated observation value and the observation values of the plurality of sensors is calculated as an observation residual, and the observation A Kalman filter that executes an operation of updating the state vector based on a residual and the state vector, and represents the strength of the geomagnetism using the first variable and takes a value larger than the first value. When the function is the first function, the second variable is used to represent the dip of geomagnetism, and the function that takes a value larger than the second value and smaller than the third value is the second function, First variable and previous A second variable, characterized in that it is a component of the state vector.
この発明によれば、カルマンフィルタを演算することにより、地磁気の強さを表す第1関数の変数(第1変数)と地磁気の伏角を表す第2関数の変数(第2変数)とを要素として含む状態ベクトルを、観測残差を用いて更新し、真の値に近づける。
カルマンフィルタを用いた演算を行う場合、観測値に重畳しているノイズの影響により、算出される状態ベクトルを構成する状態変数が、真の値から大きく外れた値として算出される場合がある。状態変数が、一度、真の値から大きく外れた値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、当該状態変数は真の値に近づかない可能性が高い。
地磁気の強さは、地球上のどの位置で観測しても、ある値以上の値を有している。よって、地磁気の強さが当該ある値以下の値となる場合には、真の値から大きく外れた値であると言える。従って、カルマンフィルタの演算により磁気の強さが当該ある値以下の値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の強さが、当該ある値以上の正しい値に復元される可能性は低い。
この発明によれば、地磁気の強さを第1の値よりも大きな値となる第1関数で表す。従って、非線形カルマンフィルタを用いた演算の結果として第1変数がどのような値に変化しても、地磁気の強さが、真の値から大きく外れた第1の値以下の値として算出されることは無い。これにより、地磁気の強さが、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。
According to the present invention, by calculating the Kalman filter, a variable of the first function (first variable) representing the strength of geomagnetism and a variable of the second function (second variable) representing the dip angle of geomagnetism are included as elements. Update the state vector with the observation residuals to bring it closer to the true value.
When performing an operation using the Kalman filter, the state variable constituting the calculated state vector may be calculated as a value greatly deviating from the true value due to the influence of noise superimposed on the observed value. If the state variable is once calculated as a value greatly deviating from the true value, there is a high possibility that the state variable will not approach the true value even if the calculation by the Kalman filter is repeated thereafter.
The strength of the geomagnetism has a value greater than a certain value at any position on the earth. Therefore, it can be said that when the strength of the geomagnetism is less than or equal to the certain value, it is a value greatly deviating from the true value. Therefore, when the magnetic strength is calculated as a value less than or equal to the certain value by the Kalman filter calculation, the magnetic strength is restored to a correct value that is greater than or equal to the certain value even if the Kalman filter calculation is repeated thereafter. It is unlikely to be done.
According to the present invention, the strength of geomagnetism is represented by a first function that is larger than the first value. Therefore, no matter what value the first variable changes as a result of the calculation using the nonlinear Kalman filter, the strength of the geomagnetism is calculated as a value less than or equal to the first value that is significantly different from the true value. There is no. Thereby, it is possible to prevent the geomagnetism intensity from being calculated as a value greatly deviating from the true value, and to prevent an inaccurate geomagnetism value from being calculated.
また、地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地磁気の伏角は、地球上のどの位置で観測しても、ある範囲の値となる。よって、カルマンフィルタの演算により、地磁気の伏角が当該ある範囲を超えた値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の伏角が当該ある範囲内の正しい値に復元される可能性は低い。
この発明によれば、地磁気の伏角を、第2の値よりも大きく且つ第3の値よりも小さな値をとる第2関数で表す。従って、非線形カルマンフィルタを用いた演算の結果として第2変数がどのような値に変化しても、地磁気の伏角が、真の値から大きく外れた第2の値よりも小さな値、または、第3の値よりも大きな値として算出されることは無い。これにより、地磁気の伏角が、真の値から大きく外れた値として算出することを防ぎ、その結果として不正確な地磁気の値を算出することを防止することができる。
Further, the geomagnetism is a magnetic field having a horizontal component toward the north of the magnetic pole and a vertical component in the dip direction, and the dip angle of the geomagnetism takes a value within a certain range no matter where the earth is observed. Therefore, when the geomagnetic dip angle is calculated as a value exceeding the certain range by the Kalman filter operation, the magnetic dip angle is restored to the correct value within the certain range even if the Kalman filter operation is repeated thereafter. Is unlikely.
According to the present invention, the dip angle of geomagnetism is represented by a second function that takes a value larger than the second value and smaller than the third value. Therefore, no matter what value the second variable changes as a result of the calculation using the nonlinear Kalman filter, the geomagnetic dip angle is smaller than the second value greatly deviating from the true value, or the third It is not calculated as a value larger than the value of. Thereby, it is possible to prevent the dip angle of geomagnetism from being calculated as a value greatly deviating from the true value, and as a result, it is possible to prevent calculating an incorrect geomagnetic value.
また、上述した地磁気測定装置において、前記第1変数をρとし、前記第2変数をηとしたとき、前記第1関数は、exp(ρ)で表され、前記第2関数は、tan−1ηで表されることが好ましい。 In the geomagnetic measurement apparatus described above, when the first variable is ρ and the second variable is η, the first function is expressed by exp (ρ), and the second function is tan −1. It is preferable to be represented by η.
この発明によれば、地磁気の強さを、第1変数ρを用いた第1関数exp(ρ)として表すため、変数ρがいかなる値に変化しても、exp(ρ)>0が満たされる。
地磁気の強さは、正の値である必要がある。よって、地磁気の強さが負の値となる場合には、真の値から大きく外れた値であると言える。従って、カルマンフィルタの演算により磁気の強さが負の値として算出される場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の強さが正の値に復元される可能性は低い。
この発明によれば、地磁気の強さを、「0」よりも大きな値の第1関数exp(ρ)で表すため、カルマンフィルタを用いた演算の結果として第1変数ρがどのような値に変化しても、地磁気の強さが、真の値から大きく外れた「0」以下の値として算出されることは無い。これにより、地磁気の強さが、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。
According to the present invention, since the strength of geomagnetism is expressed as the first function exp (ρ) using the first variable ρ, exp (ρ)> 0 is satisfied regardless of the value of the variable ρ. .
The strength of geomagnetism needs to be a positive value. Therefore, it can be said that when the strength of the geomagnetism is a negative value, the value is greatly deviated from the true value. Therefore, when the magnetic strength is calculated as a negative value by the calculation of the Kalman filter, the possibility that the magnetic strength is restored to a positive value is low even if the calculation by the Kalman filter is repeated thereafter.
According to the present invention, since the strength of geomagnetism is expressed by the first function exp (ρ) having a value larger than “0”, the value of the first variable ρ varies as a result of the calculation using the Kalman filter. Even so, the strength of the geomagnetism is not calculated as a value of “0” or less that is significantly different from the true value. Thereby, it is possible to prevent the geomagnetism intensity from being calculated as a value greatly deviating from the true value, and to prevent an inaccurate geomagnetism value from being calculated.
同様に、この発明によれば、地磁気の伏角を、第2変数ηを用いた第2関数tan−1ηとして表すため、変数ηがいかなる値に変化しても、−π/2<tan−1η<π/2が満たされる。
地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界であり、地磁気の伏角は、北の磁極に近づくに従って、+90度に近づき、南の磁極に近づくに従って、−90度に近づく。つまり、地磁気の伏角は、−π/2よりも大きく、且つ、π/2よりも小さな値となる必要がある。よって、カルマンフィルタの演算により、地磁気の伏角が、−π/2以下の値、または、π/2以上の値として算出された場合には、その後、カルマンフィルタによる演算を繰り返しても、磁気の伏角が−π/2よりも大きく、且つ、π/2よりも小さな正しい値に復元される可能性は低い。
この発明によれば、地磁気の伏角を、−π/2よりも大きく且つπ/2よりも小さな値をとる第2関数tan−1ηで表すため、カルマンフィルタを用いた演算の結果として第2変数ηがどのような値に変化しても、地磁気の伏角が、真の値から大きく外れた−π/2以下の値、または、π/2以上の値として算出されることは無い。これにより、地磁気の伏角が、真の値から大きく外れた値として算出することを防ぎ、その結果として不正確な地磁気の値を算出することを防止することができる。
Similarly, according to the present invention, since the dip angle of the geomagnetism is expressed as the second function tan −1 η using the second variable η, even if the variable η changes to any value, −π / 2 <tan − 1 η <π / 2 is satisfied.
The geomagnetism is a magnetic field having a horizontal component toward the north of the magnetic pole and a vertical component in the direction of the dip. The dip of the geomagnetism approaches +90 degrees as it approaches the north magnetic pole, and decreases to -90 degrees as it approaches the south magnetic pole. Get closer. That is, the dip angle of geomagnetism needs to be larger than −π / 2 and smaller than π / 2. Therefore, if the geomagnetic dip angle is calculated as a value of −π / 2 or less or a value of π / 2 or more by the calculation of the Kalman filter, the magnetic dip angle is reduced even if the calculation by the Kalman filter is repeated thereafter. It is unlikely to be restored to a correct value that is greater than -π / 2 and smaller than π / 2.
According to the present invention, the dip angle of the geomagnetism is expressed by the second function tan −1 η that takes a value larger than −π / 2 and smaller than π / 2. Regardless of the value of η, the dip angle of geomagnetism is not calculated as a value of −π / 2 or less, or a value of π / 2 or more greatly deviating from the true value. Thereby, it is possible to prevent the dip angle of geomagnetism from being calculated as a value greatly deviating from the true value, and as a result, it is possible to prevent calculating an incorrect geomagnetic value.
次に、本発明に係る地磁気測定方法は、システムを観測して観測値を各々出力する複数のセンサを備える状態推定装置に用いられる状態推定方法であって、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新し、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素である
ことを特徴とする。
Next, a geomagnetism measurement method according to the present invention is a state estimation method used in a state estimation device that includes a plurality of sensors that observe a system and output observation values, respectively, and uses the state transition model of the system. A state vector including a plurality of state variables indicating states is estimated, an estimated observation value is calculated from the state vector using an observation value model, and a difference between the estimated observation value and the observation values of the plurality of sensors is calculated. Calculated as an observation residual, updates the state vector based on the observation residual and the state vector, represents the strength of geomagnetism using the first variable, and takes a value larger than the first value When the function is the first function, the second variable is used to represent the dip of geomagnetism, and the function that takes a value larger than the second value and smaller than the third value is the second function, First variable and second variable But it characterized in that it is a component of the state vector.
この発明によれば、地磁気の強さ及び伏角が、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。 According to the present invention, it is possible to prevent the strength and the dip angle of the geomagnetism from being calculated as values greatly deviating from the true values, and to prevent the calculation of inaccurate geomagnetism values.
次に、本発明にかかる地磁気測定装置に用いられる地磁気測定プログラムは、システムを観測して観測値を各々出力する複数のセンサを備える地磁気測定装置に用いられる地磁気測定プログラムであって、状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定する処理と、観測値モデルを用いて前記状態ベクトルから推定観測値を算出する処理と、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出する処理と、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する処理とを、コンピュータに実行させ、第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素である
ことを特徴とする。
Next, a geomagnetism measurement program used in a geomagnetism measurement apparatus according to the present invention is a geomagnetism measurement program used in a geomagnetism measurement apparatus including a plurality of sensors that observe a system and output observation values, respectively, and a state transition model A process for estimating a state vector having a plurality of state variables indicating the state of the system as elements, a process for calculating an estimated observation value from the state vector using an observation value model, the estimated observation value, and the A process for calculating a difference from observation values of a plurality of sensors as an observation residual and a process for updating the state vector based on the observation residual and the state vector are executed by a computer, and the first variable is set to The function that represents the strength of the geomagnetism, the function that takes a value larger than the first value as the first function, and the second variable is used to represent the dip angle of the geomagnetism. A function that takes a larger and smaller than the third value than the value, when the second function, and the first variable and the second variable, characterized in that it is a component of the state vector.
この発明によれば、地磁気の強さ及び伏角が、真の値から大きく外れた値として算出されることを防ぎ、不正確な地磁気の値を算出することを防止することができる。 According to the present invention, it is possible to prevent the strength and the dip angle of the geomagnetism from being calculated as values greatly deviating from the true values, and to prevent the calculation of inaccurate geomagnetism values.
<A.実施形態>
本発明の実施の形態について図面を参照して説明する。
<A. Embodiment>
Embodiments of the present invention will be described with reference to the drawings.
[1. 機器構成及びソフトウェア構成]
図1は、本発明の実施形態に係る携帯電話機のブロック図であり、図2は携帯電話機の外観を示す斜視図である。携帯電話機1は、携帯電話機1の姿勢を変えることにより変化する画面の向く方向に応じて地図などの画像を回転させることによって、画像によって表されている方位を現実空間の方位に追従させる機能を備える。この機能は、各種センサの出力に基づいてカルマンフィルタの演算を行い、携帯電話機1の姿勢を推定することによって実現される。
[1. Device configuration and software configuration]
FIG. 1 is a block diagram of a mobile phone according to an embodiment of the present invention, and FIG. 2 is a perspective view showing an appearance of the mobile phone. The
携帯電話機1は、各種の構成要素とバスを介して接続され装置全体を制御するCPU10、CPU10の作業領域として機能するRAM20、各種のプログラムやデータを記憶したROM30、通信を実行する通信部40、画像を表示する表示部50、及びGPS部60を備える。GPS部60は、GPS衛星からの信号を受信して携帯電話機1の位置情報(緯度、経度)を生成するものである。地磁気を検出して地磁気データを出力する地磁気検出部70、加速度を検出して加速度データを出力する加速度検出部80、角速度を検出して角速度データを出力する角速度検出部90を備える。
The
地磁気検出部70は、X軸地磁気センサ71、Y軸地磁気センサ72、及びZ軸地磁気センサ73を備える。各センサは、MI素子(磁気インピーダンス素子)、MR素子(磁気抵抗効果素子)などを用いて構成することができる。地磁気センサI/F74は、各センサからの出力信号をAD変換して地磁気データを出力する。
この地磁気データは、x軸、y軸およびz軸の3成分によって、磁極北に向かう水平成分と伏角方向の鉛直成分とを有するベクトルを示すデータである。なお、地磁気検出部70は、携帯電話機1に設けられるので、携帯電話機1の内部の部品(スピーカなど)が出す磁気と、携帯電話機1の外部にある外部物が出す磁気と、実際の地磁気とが合成された磁界を検出する。しかしながら、後述する地磁気測定プログラム100で想定している状態遷移モデルでは、外部物が出す磁気は無視できる程小さいものとする。
The
This geomagnetic data is data indicating a vector having a horizontal component toward the magnetic pole north and a vertical component in the dip direction by three components of the x-axis, y-axis, and z-axis. In addition, since the
加速度検出部80は、X軸加速度センサ81、Y軸加速度センサ82、及びZ軸加速度センサ83を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサI/F84は、各センサからの出力信号をAD変換して加速度データを出力する。
この加速度データは、加速度検出部80と一体となって運動する物体固定の系における慣性力と重力との合力を、x軸、y軸及びz軸の3成分を有するベクトルとして示すデータである。したがって、携帯電話機1が静止状態または等速直線運動状態にあれば、加速度データは画面に対して固定されたxyz座標空間において重力加速度の大きさと方向とを示すベクトルデータである。
The
This acceleration data is data indicating the resultant force of inertial force and gravity in an object-fixed system that moves integrally with the
角速度検出部90は、X軸角速度センサ91、Y軸角速度センサ92、及びZ軸角速度センサ93を備える。角速度センサI/F94は、各センサからの出力信号をAD変換して角速度データを出力する。角速度データは、各軸の回りの角速度を示す。
The angular
CPU10は、ROM30に格納されている地磁気測定プログラム100を実行することによって、動的システムの状態を表す複数の物理量、例えば、携帯電話機1の姿勢や、地磁気の強さや伏角等を推定する。すなわち、携帯電話機1は、動的システムの状態を表す複数の物理量の推定値に基づいて地磁気の正確な値を算出する、地磁気測定装置として機能する。
地磁気測定プログラム100は、初期値生成モジュール110と、非線形カルマンフィルタモジュール120とを備える。非線形カルマンフィルタモジュール120は、非線形カルマンフィルタKFの演算を実行する。
The
The
非線形カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムの有する複数のセンサが計測する値(観測値)を推定する観測モデルと、を有する。
そして、非線形カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定し、推定された状態ベクトルに基づいて複数の観測値を要素とする観測値ベクトルを推定する。ここで、観測値ベクトルとは、地磁気検出部70から出力される地磁気データの値、加速度検出部80から出力される加速度データの値、及び角速度検出部90から出力される角速度データの値を要素とするベクトルである。
次に、非線形カルマンフィルタは、推定された観測値ベクトルと、地磁気検出部70、加速度検出部80、及び角速度検出部90からの実際の出力を要素とする観測値ベクトルとの差分として算出される観測残差に基づいて、状態ベクトルをより真値に近い値へと更新する。非線形カルマンフィルタは、以上のような演算を繰り返すことにより、動的システムの状態を表す複数の物理量の推定値を、より実際の値(真値)に近い正確な値へと更新する。
本実施形態における非線形カルマンフィルタは、動的システムの状態を表す物理量として、携帯電話機1の姿勢を表すクォータニオンq、地磁気の強さr、地磁気の伏角θ、携帯電話機1の角速度ω、角速度センサ91〜93のオフセット推定値go、及び地磁気センサ71〜73のオフセット推定値moを採用し、これらを推定の対象とする。
The nonlinear Kalman filter is a state transition model that estimates changes over time in a plurality of physical quantities representing the state of a dynamic system, an observation model that estimates values (observation values) measured by a plurality of sensors in the dynamic system, Have
The nonlinear Kalman filter estimated the state vector after a unit time from a state vector whose elements are a plurality of physical quantities representing the state of the dynamic system at a certain time, using the state transition model. An observation value vector having a plurality of observation values as elements is estimated based on the state vector. Here, the observed value vector includes the value of the geomagnetic data output from the
Next, the nonlinear Kalman filter is an observation calculated as a difference between the estimated observation value vector and an observation value vector whose elements are actual outputs from the
The nonlinear Kalman filter in the present embodiment is a quaternion q representing the attitude of the
ところで、非線形カルマンフィルタを用いた演算を繰り返す場合、観測値に重畳しているノイズの影響や、状態遷移モデル及び観測モデルが有する非線形性の影響により、推定される物理量が実際の値から大きく外れた値として算出される場合がある。この場合には、その後、非線形カルマンフィルタによる演算を繰り返しても、真の値に近い物理量を推定することは難しい。
そこで、本実施形態では、動的システムの状態を表す複数の物理量のうち、値の範囲を定めることのできる物理量は、当該範囲外の値に変化しないような状態変数を用いて表す。そして、複数の物理量の各々に対応する状態変数を要素とするような状態ベクトルxkを定め、カルマンフィルタの演算を実行する。以下において、本実施形態に係る状態変数について述べる。
By the way, when the calculation using the nonlinear Kalman filter is repeated, the estimated physical quantity greatly deviates from the actual value due to the influence of noise superimposed on the observation value and the nonlinearity of the state transition model and the observation model. It may be calculated as a value. In this case, it is difficult to estimate a physical quantity close to the true value even if the calculation by the nonlinear Kalman filter is repeated thereafter.
Therefore, in the present embodiment, among a plurality of physical quantities representing the state of the dynamic system, a physical quantity that can define a value range is represented using a state variable that does not change to a value outside the range. Then, determine the state vector x k such that a state variable corresponding to each of a plurality of physical quantity as elements, performing the calculation of the Kalman filter. In the following, state variables according to the present embodiment will be described.
まず、動的システムの状態を表す複数の物理量のうち、地磁気の強さrと、地磁気の伏角θについて検討する。
前述の通り、地磁気は、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する磁界である。具体的には、地磁気の伏角は、北の磁極に近づくに従って、+90度(第3の値)に近づき、南の磁極に近づくに従って、−90度(第2の値)に近づく。つまり、地磁気の伏角θは、−π/2<θ<π/2を満たす。また、地磁気の強さrは、0(第1の値)よりも大きな値となる。
そこで、本実施形態では、地磁気の強さrを、変数ρを用いて式(1)に示す関数(第1関数)exp(ρ)として表し、この変数ρを状態変数とする。また、地磁気の伏角θを、変数ηを用いて式(2)に示す関数(第2関数)tan−1ηとして表し、この変数ηを状態変数とする。
この場合、変数ρがいかなる値に変化しても、exp(ρ)>0が満たされる。すなわち、非線形カルマンフィルタの演算を繰り返した場合であっても、地磁気の強さrは、r>0という条件を満たすことになる。また、変数ηがいかなる値に変化しても、−π/2<tan−1η<π/2が満たされる。すなわち、非線形カルマンフィルタの演算を繰り返した場合であっても、地磁気の伏角θは、−π/2<θ<π/2という条件を満たすことになる。
As described above, the geomagnetism is a magnetic field having a horizontal component toward the magnetic pole north and a vertical component in the dip direction. Specifically, the dip angle of geomagnetism approaches +90 degrees (third value) as it approaches the north magnetic pole, and approaches -90 degrees (second value) as it approaches the south magnetic pole. That is, the dip angle θ of geomagnetism satisfies −π / 2 <θ <π / 2. Further, the strength r of geomagnetism is a value larger than 0 (first value).
Therefore, in this embodiment, the strength r of geomagnetism is expressed as a function (first function) exp (ρ) shown in Expression (1) using a variable ρ, and this variable ρ is a state variable. Further, the dip angle θ of geomagnetism is expressed as a function (second function) tan −1 η shown in Expression (2) using a variable η, and this variable η is a state variable.
In this case, exp (ρ)> 0 is satisfied regardless of the value of the variable ρ. That is, even when the calculation of the nonlinear Kalman filter is repeated, the geomagnetic strength r satisfies the condition r> 0. Moreover, even if the variable η changes to any value, −π / 2 <tan −1 η <π / 2 is satisfied. That is, even when the calculation of the nonlinear Kalman filter is repeated, the geomagnetic dip angle θ satisfies the condition of −π / 2 <θ <π / 2.
携帯電話機1の姿勢を表すクォータニオンqは、物体の姿勢(回転状態)を表す数学的表現であって、4次元数で与えられる。ここで、地面に固定された座標系での地磁気ベクトルと重力ベクトルがそれぞれme、geであるとする。物体が静止していると仮定したとき、センサ固定の座標系で磁気センサと加速度センサの出力がそれぞれme、geとなる姿勢を基準姿勢とし、その基準姿勢を表すクォータニオンをq=(0、0、0、1)と定める。任意の姿勢は基準姿勢に対して単位ベクトルαの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢を表すクォータニオンqは以下に示す式(3)で与えられる。
携帯電話機1は、任意の姿勢を取り得るので、姿勢を表すクォータニオンqの値の範囲を制限することができない。従って、姿勢を表すクォータニオンqは、値に制限を加えることなく、そのまま状態変数として採用する。
Since the
また、角速度ω、角速度センサのオフセット推定値go、及び地磁気センサのオフセット推定値moについても、任意の値を取り得るため、値に制限を加えることなく、そのまま状態変数として採用する。
なお、状態変数に地磁気センサのオフセット推定値mo及び角速度センサのオフセット推定値goを含ませたのは、地磁気データにはセンサ71〜73のオフセットが重畳し、角速度データにはセンサ91〜93のオフセットが重畳するからである。例えば、携帯電話機1には磁石を備えたスピーカが内蔵されているため、地磁気センサは磁石の磁力を常時検出してしまう。あるいは、温度変化によってもオフセットが変動する。動的システムの状態を表す複数の物理量を正確に推定するためには、オフセット推定値moにより補正された地磁気データ及びオフセット推定値goにより補正された角速度データを用いる必要がある。
Also, the angular velocity ω, the estimated offset value g o of the angular velocity sensor, and the estimated offset value m o of the geomagnetic sensor can take arbitrary values, and thus are directly adopted as state variables without limiting the values.
The state variable includes the estimated offset value m o of the geomagnetic sensor and the estimated offset value g o of the angular velocity sensor because the offset of the sensors 71 to 73 is superimposed on the geomagnetic data, and the
以上のように定めた状態変数を要素とする時刻kにおける状態ベクトルxkは、以下の式(4)で表される。
次に、カルマンフィルタにおいて観測の対象となるのは、3軸の地磁気センサ71〜73の出力である観測値m、3軸の加速度センサ81〜83の出力である観測値a、及び3軸の角速度センサ91〜93の出力である観測値gである。観測値m、a、gは各々X軸、Y軸及びZ軸の3つの要素からなる観測値ベクトルyである。時刻kにおける観測値ベクトルykは式(5)で与えられる。
非線形カルマンフィルタKFは、このような、状態ベクトルxk及び観測値ベクトルykを用いて、動的システムの状態を表す複数の物理量を推定する。具体的には、非線形カルマンフィルタKFは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、現在時刻(時刻k−1)のシステムの状態を示す状態ベクトルxk−1から単位時間が経過した後(時刻k)の状態ベクトルxkを推定する。次に、動的システムの状態から各種センサの観測値を推定する観測モデルを用いて、推定された状態ベクトルxkから、観測値ベクトルykを推定する。そして、推定された観測値ベクトル(推定観測値)と、各種センサの出力(実際の観測値)を表す観測値ベクトルykとの差分を観測残差ekとして算出し、観測残差ekに基づいてカルマンゲインKkを算出する。このようにして算出された観測残差ekと、カルマンゲインKkとに基づいて、非線形カルマンフィルタKFは、状態ベクトルxkをより正確な値に更新する。 The nonlinear Kalman filter KF estimates a plurality of physical quantities representing the state of the dynamic system using the state vector x k and the observed value vector y k . Specifically, the nonlinear Kalman filter KF uses a state transition model that represents a change in the state of the dynamic system over time, from a state vector x k−1 that indicates the state of the system at the current time (time k−1). The state vector x k after the unit time has elapsed (time k) is estimated. Next, an observation value vector y k is estimated from the estimated state vector x k using an observation model that estimates the observation values of various sensors from the state of the dynamic system. Then, the estimated observed value vector (estimated observed values), calculates the difference between the observed value vector y k representing the output of various sensors (the actual observed value) as the observation residuals e k, observation residuals e k Kalman gain Kk is calculated based on Based on the observation residual e k calculated in this way and the Kalman gain K k , the nonlinear Kalman filter KF updates the state vector x k to a more accurate value.
初期値生成モジュール110は、時刻k=0における状態変数を要素とする初期値INI(状態ベクトル)を算出し、非線形カルマンフィルタモジュール120に対して出力する。初期値INIは、姿勢を表すクォータニオンqの初期値q0、変数ρの初期値ρ0、変数ηの初期値η0、角速度ωの初期値ω0、角速度センサのオフセット推定値goの初期値go,0、及び地磁気センサのオフセット推定値moの初期値mo,0を含む。
初期値INIは、状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下で説明するような値に設定しても良い。
The initial
The initial value INI may be set as appropriate so that the state variable converges to an accurate value as soon as possible. For example, the initial value INI may be set as described below.
地磁気の強さrを算出するための変数ρの初期値ρ0、及び地磁気の伏角θを算出するための変数ηの初期値η0は、例えば、GPS部60から供給される位置情報に基づいて生成する。これは、地球上の位置が特定できれば、その位置における地磁気の強さr及び伏角θを知ることができるからである。具体的には、初期値生成モジュール110は、ROM30に位置情報と地磁気の強さr及び伏角θとを対応づけて記憶したルックアップテーブルLUT1を格納しておき、これを参照して位置情報に対応する地磁気の強さr及び伏角θを読み出す。そして、初期値生成モジュール110は、位置情報に対応する地磁気の強さrに基づいて初期値ρ0を算出すると共に、位置情報に対応する伏角θに基づいて初期値η0を算出する。なお、携帯電話機1が衛星からの電波の届かない場所(例えば、地下街)にある場合には、GPS部60から位置情報が得られない。そのような場合には、携帯電話機1が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルLUT1に格納されている。初期値生成モジュール110は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルLUT1から読み出す。例えば、日本で販売された携帯電話機1については、明石市の地磁気の強さr及び伏角θに基づいて、初期値ρ0及び初期値η0を算出すればよい。
The initial value ρ 0 of the variable ρ for calculating the strength r of geomagnetism and the initial value η 0 of the variable η for calculating the dip angle θ of geomagnetism are based on, for example, position information supplied from the
角速度ωの初期値の初期値ω0は、例えば、携帯電話機1が静止していることを仮定して、「0」に設定する。角速度センサのオフセット推定値g。の初期値go,0についても、例えば、携帯電話機1が静止していることを仮定して、「0」に設定する。姿勢を表すクォータニオンqの初期値q0に関しては、例えば、携帯電話機1が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を、初期値INIに設定する。
The initial value ω 0 of the initial value of the angular velocity ω is set to “0” on the assumption that the
地磁気センサのオフセット推定値moの初期値mo,0は、例えば、時刻k=0の地磁気センサの観測値m0、携帯電話機1を使用する地域の地磁気ベクトルmtypical、及び、式(7)に示す行列B(q)を用いて、以下に示す式(6)で与えられる値に設定する。ここで、行列B(q)は、地面に固定された座標系を、姿勢qを用いて携帯電話機1の地磁気センサに固定された座標系に変換するために用いられる行列であり、以下に示す式(7)で与えられる。なお、ROM30には、位置情報(経度、緯度)と地磁気ベクトルmtypicalとを対応づけて記憶したルックアップテーブルLUT2が記憶されている。初期値生成モジュール110は、GPS部60で生成される位置情報に基づいてルックアップテーブルLUT2を参照して地磁気ベクトルmtypicalを取得し、式(6)を演算することによって、地磁気センサのオフセット推定値moの初期値mo,0を得る。
以上ように、初期値生成モジュール110は、初期値INIを、非線形カルマンフィルタKFに対して出力する。
As described above, the initial
[2. 非線形カルマンフィルタ]
次に、非線形カルマンフィルタKFの内容について説明する。
[2. Nonlinear Kalman filter]
Next, the content of the nonlinear Kalman filter KF will be described.
[2.1. 非線形カルマンフィルタの概要]
図3は、非線形カルマンフィルタKFの機能ブロック図である。非線形カルマンフィルタKFのシステムの状態遷移モデルは以下に示す式(8)で与えられ、観測モデルは以下に示す式(9)で与えられる。
FIG. 3 is a functional block diagram of the nonlinear Kalman filter KF. The state transition model of the system of the nonlinear Kalman filter KF is given by the following equation (8), and the observation model is given by the following equation (9).
ここで、式(8)に現れる状態ベクトルxkはn次元のベクトルであり、式(9)に現れる観測値ベクトルykはm次元のベクトルである(本実施形態では、n=15、m=9である)。式(4)で示したように状態ベクトルxkは、姿勢を表すクォータニオンqkや地磁気センサのオフセット推定値mo,k等の状態変数を要素とする。プロセスノイズwkと観測ノイズυkは0を中心とするガウスノイズである。
式(8)は、非線形カルマンフィルタKFの状態遷移モデルを表す。式(8)は、時刻kにおける状態ベクトルxkが、時刻k−1における状態ベクトルxk−1を関数fに代入し、その結果と時刻k−1におけるプロセスノイズwk−1とを加算したものであることを示している。
また、式(9)は、非線形カルマンフィルタKFの観測モデルを表す。式(9)は、時刻kにおける観測値ベクトルykが、時刻kにおける状態ベクトルxkを関数hに代入し、その結果と時刻kにおける観測ノイズυkとを加算したものであることを示している。なお、以下の説明では、プロセスノイズwkの共分散をQk、観測ノイズυkの共分散をRkとする。
Here, the state vector x k appearing in Equation (8) is an n-dimensional vector, and the observed value vector y k appearing in Equation (9) is an m-dimensional vector (in this embodiment, n = 15, m = 9). As shown in the equation (4), the state vector x k includes state variables such as the quaternion q k representing the posture and the estimated offset value m o, k of the geomagnetic sensor. The process noise w k and the observation noise υ k are Gaussian noises centered on zero.
Equation (8) represents a state transition model of the nonlinear Kalman filter KF. Equation (8), the state vector x k at time k is, by substituting the state vector x k-1 at time k-1 to function f, adding the process noise w k-1 at the result and time k-1 It is shown that
Equation (9) represents an observation model of the nonlinear Kalman filter KF. Equation (9) indicates that the observed value vector y k at time k is obtained by substituting the state vector x k at time k into the function h and adding the result to the observed noise υ k at time k. ing. In the following description, the covariance of the process noise w k is Q k , and the covariance of the observation noise υ k is R k .
観測残差ekは、時刻kの観測値ベクトルykと、観測モデルを用いて推定された観測値ベクトルとの差分であり、以下に示す式(10)で表すことができる。
また、非線形カルマンフィルタは、観測残差ekと、式(11)に示すカルマンゲインKkとを用いて、以下の式(12)に示すように、状態ベクトルの推定値を更新し、以下の式(13)に示すように、状態ベクトルの推定誤差の共分散を更新する。
式(10)に示すように、観測モデルを用いて推定された観測値ベクトルは、状態ベクトルxkの推定値を、観測モデルに適用して算出される。従って、観測値ベクトルykと、観測モデルを用いて推定された観測値ベクトルとの差分である観測残差ekにより、状態ベクトルxkが実際の値に近い適切な値に収束したか否かを判断することができる。
Further, the nonlinear Kalman filter updates the state vector estimation value as shown in the following equation (12) using the observation residual e k and the Kalman gain K k shown in the equation (11). As shown in Expression (13), the state vector estimation error covariance is updated.
As shown in Expression (10), the observed value vector estimated using the observation model is calculated by applying the estimated value of the state vector x k to the observation model. Thus, the observed value vector y k, the observation residuals e k is the difference between the observed value vector estimated by using the observation model, or the state vector x k has converged to an appropriate value close to the actual value not Can be determined.
式(10)の減算は、図3に示す減算器260で実行される。また、式(12)の右辺第2項の演算は、カルマンゲイン付与部270において実行され、式(12)の加算は加算器280において実行される。遅延部290は、加算器280の出力を次の時刻まで遅延させる。
The subtraction of Expression (10) is executed by the
[2.2. シグマポイントカルマンフィルタによる演算]
なお、本実施形態では、非線形カルマンフィルタKFは、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。
以下では、シグマポイントカルマンフィルタを用いる場合の、状態ベクトルの更新、状態ベクトルの推定誤差の算出、観測値ベクトルの推定値の算出、及び相互相関行列の算出について、具体的に説明する。
[2.2. Calculation with Sigma Point Kalman Filter]
In the present embodiment, the nonlinear Kalman filter KF is configured by a sigma point Kalman filter using unscented transformation.
Hereinafter, update of the state vector, calculation of the state vector estimation error, calculation of the observation vector estimation value, and calculation of the cross-correlation matrix when the sigma point Kalman filter is used will be described in detail.
シグマポイントカルマンフィルタでは、n行n列の共分散行列を用いて、最初に(2n+1)個のシグマポイントを生成する。まず、複数のシグマポイントの平均に対して、平均のまわりのシグマボイントの広がりを表すスケーリングパラメータλを用いて、式(14)及び式(15)を定義する。
このとき、シグマポイントは、式(16)及び式(17)で決定される。式(14)乃至式(17)の演算は、シグマポイント生成部210によって実行される。なお、シグマポイント生成部210には、初期値INIが供給される。シグマポイントカルマンフィルタの初回の演算では、初期値INIを式(16)の右辺に用いてシグマポイント生成部210はシグマポイントを生成する。
状態遷移モデル部220は、ある時刻k−1のシグマポイントから、単位時間が経過した後の時刻kのシグマポイントを推定する。この演算は式(18)で与えられる。
また、推定される状態ベクトルの平均は、以下に示す式(19)で与えられる。この演算は平均算出部230において実行される。
また、推定される状態ベクトルの誤差の共分散は、以下に示す式(20)で与えられる。
次に、推定される観測値は、以下に示す式(21)で与えられる。γk(i)の演算は観測モデル部240において実行され、式(21)の演算は平均処理部250において実行される。
ここで、残差の共分散は、以下に示す式(22)で与えられる。
また、相互相関行列は式(23)で与えられる。
[2.3. 状態遷移モデル]
次に、状態遷移モデル部220の演算で用いる状態遷移モデルについて説明する。
本実施形態における姿勢推定モデルでは、以下に示す式(24)のように、状態ベクトルxkを構成する状態変数のうち、姿勢を表すクォータニオンq以外は、前の時刻から変化しないと仮定する。但し、実際にはカルマンゲインKkと観測残差ekとによって、次の状態ベクトルxkが補正されるので、値が全く変化しないわけではなく、真値に近づいてゆく。
Next, the state transition model used in the calculation of the state
The orientation estimation model in the present embodiment, as in equation (24) shown below, among the state variables that constitute the state vector x k, it is assumed that except quaternion q representing the orientation does not change from the previous time. However, since the next state vector x k is actually corrected by the Kalman gain K k and the observation residual e k , the value does not change at all and approaches the true value.
ここで、姿勢の回転を表す演算子Ωを、以下の式(25)で定義する。
但し、I3×3を3行3列の単位行列とし、また、3次元ベクトルu=(u1,u2,u3)に対して、[u×]という記号を式(26)で定義する。また、ΔTを、測定時間間隔として、演算子Ωを構成する成分を、式(27)で定義する。
However, I 3 × 3 is a 3 × 3 unit matrix, and the symbol [u ×] is defined by the equation (26) for the three-dimensional vector u = (u1, u2, u3). Further, with ΔT as a measurement time interval, a component constituting the operator Ω is defined by Expression (27).
このとき、ある時刻k−1の姿勢を表すクォータニオンqk−1から、単位時間が経過した後の時刻kの姿勢を表すクォータニオンqkを推定する演算は、式(28)により表される。
姿勢を表すクォータニオンqは、正規化条件||q||=1が満たされていないといけないが、シグマポイントの平均を求めるとその条件が満たされなくなってしまう。クォータニオンqに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化するようにすればそのような問題は解決される。
より厳密に正規化条件を保つためには、状態ベクトルのうち姿勢についてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。これにより、システム全体として姿勢を推定する装置として機能する。
The quaternion q representing the posture must satisfy the normalization condition || q || = 1. However, when the average of the sigma points is obtained, the condition is not satisfied. When any operation is performed on the quaternion q, such a problem can be solved by normalizing the result after the operation with the size of the vector itself.
In order to maintain the normalization condition more strictly, the posture of the state vector is limited to only the difference information from the previous time using MRPs (modified Rodrigues parameters), and the posture information outside the Kalman filter is obtained from the Kalman filter. Update may be performed based on the difference information. As a result, the entire system functions as a device for estimating the posture.
[2.4. 観測モデル]
次に、観測モデル部240で実行する観測モデルの演算について説明する。
角速度センサ91〜93の出力である観測値gの推定値γgyroは、角速度ωと、角速度センサのオフセット推定値goとを用いて、式(29)で与えられる。
Next, the calculation of the observation model executed by the
Estimate gamma Gyro observations g is the output of the
地面に固定された水平東、水平北、鉛直上の順の座標系での地磁気ベクトルは式(30)で与えられると推定している。従って地磁気センサ71〜73の出力である観測値mの推定値γmagは、式(7)の行列を用いて、式(31)で与えられる。
また、地面に固定された水平東、水平北、鉛直上の順の座標系での重力ベクトルが重力加速度で正規化されているとすると、式(32)が成立する。従って、加速度センサ81〜83の出力である観測値aの推定値γaccは、式(7)の行列を用いて、式(33)で与えられる。
従って、式(5)、式(29)、式(31)、及び式(33)を、式(10)に適用することにより、観測残差ekが式(34)で表される。
[3. 状態変数の収束]
以上に述べた本実施形態に係る非線形カルマンフィルタKFは、動的システムの状態を表す複数の物理量をそのまま状態ベクトルxkとして採用するのではなく、値の範囲を限定することのできる物理量については、当該物理量が当該範囲外の値に変化しないような状態変数により表現して、状態ベクトルxkの要素とした。
これにより、本実施形態に係る非線形カルマンフィルタKFは、状態ベクトルxkが不適切な値に収束することを防止し、動的システムの状態を表す複数の物理量について、真値に近い値を推定することが可能である。
[3. Convergence of state variables]
The nonlinear Kalman filter KF according to the present embodiment described above does not directly adopt a plurality of physical quantities representing the state of the dynamic system as the state vector x k , but for physical quantities that can limit the range of values, and the physical quantity is represented by the state variable that does not change to a value outside that range, and the elements of the state vector x k.
Thereby, the nonlinear Kalman filter KF according to the present embodiment prevents the state vector x k from converging to an inappropriate value, and estimates a value close to a true value for a plurality of physical quantities representing the state of the dynamic system. It is possible.
以下では、本実施形態に係る非線形カルマンフィルタKFの有効性を示すために、本実施形態と、動的システムの状態を表す複数の物理量をそのまま状態変数として採用した場合(以下、「対比例」と称する)とを、比較して説明する。
対比例は、以下の式(35)に示す状態ベクトルCxkを用いる。状態ベクトルCxkは、その要素として、変数ρの代わりに地磁気の強さrを含み、変数ηの代わりに地磁気の伏角θを含む点で、式(4)に示した本実施形態に係る状態ベクトルxkと相違する。
For the comparison, a state vector C x k shown in the following equation (35) is used. The state vector C x k includes, as its elements, the geomagnetic strength r instead of the variable ρ and the geomagnetic dip angle θ instead of the variable η, according to the present embodiment shown in the equation (4). It differs from the state vector x k.
非線形カルマンフィルタKFに状態ベクトルCxkを用いる場合、地磁気の強さr及び地磁気の伏角θが現実とは大きく異なる値に収束することがある。
図4に示すように、地磁気の強さrの実際の値(真値)が0.5ガウスで、地磁気の伏角θの実際の値(真値)がπ/3である場合を想定し、携帯電話機1が基準姿勢である場合を仮定した事例を用いて、本実施形態に係る状態ベクトルxkが収束する様子と、状態ベクトルCxkが真値から大きく離れた値に収束する様子とについて、説明する。
When the state vector C x k is used for the nonlinear Kalman filter KF, the geomagnetism strength r and the geomagnetic depression angle θ may converge to values that are significantly different from actual ones.
As shown in FIG. 4, it is assumed that the actual value (true value) of the geomagnetic strength r is 0.5 gauss and the actual value (true value) of the geomagnetic dip angle θ is π / 3, Using a case where the
本実施形態のように、地磁気の強さrが変数ρを用いて表される場合には、地磁気の強さrが、rの取り得る範囲(r>0)から逸脱することは無い。同様に、地磁気の伏角θが変数ηを用いて表される場合には、地磁気の伏角θが、θの取り得る範囲(−π/2<θ<π/2)から逸脱することは無い。この場合には、地磁気の強さr及び伏角θが真値に収束する可能性が高くなる。図4の事例では、例えば、状態ベクトルxkのうち、変数ρにより表される地磁気の強さrは、r=0.5ガウスに収束し、変数ηにより表される地磁気の伏角θは、θ=π/3に収束する可能性が高い。 When the geomagnetism strength r is expressed using the variable ρ as in this embodiment, the geomagnetism strength r does not deviate from the range that r can take (r> 0). Similarly, when the geomagnetic dip angle θ is expressed using the variable η, the geomagnetic dip angle θ does not deviate from a possible range of θ (−π / 2 <θ <π / 2). In this case, there is a high possibility that the geomagnetic strength r and the dip angle θ will converge to true values. In the example of FIG. 4, for example, in the state vector x k , the geomagnetic strength r represented by the variable ρ converges to r = 0.5 gauss, and the geomagnetic depression angle θ represented by the variable η is There is a high possibility of convergence to θ = π / 3.
一方、対比例では、ある瞬間k=kaにおいて、地磁気の強さr及び伏角θが真値から大きく離れた値となり、その後再び真値に近づくことなく偽値に収束する場合がある。図4の事例では、例えば、状態ベクトルCxkのうち、地磁気の強さrがr=−0.5ガウスに収束し、地磁気の伏角θがθ=−2π/3に収束する場合がある。 On the other hand, in the comparative example, there at the instant k = k a, geomagnetic strength r and dip θ becomes larger away value from the true value, then it may be re-converges to a false value without approaching the true value. In the example of FIG. 4, for example, in the state vector C x k , the geomagnetic strength r may converge to r = −0.5 gauss and the geomagnetic depression angle θ may converge to θ = −2π / 3. .
この場合、状態ベクトルxkが真値に収束した場合の観測残差と、状態ベクトルCxkが偽値に収束した場合の観測残差とは、等しい値となる。具体的には、式(36)に示す本実施形態に係る推定値γmagと、式(37)に示す対比例に係る推定値Cγmagとは、等しい値となるため、式(34)より、両者の観測残差は等しくなる。
従って、本実施形態に係る推定値γmagが、真値に近い値である場合には、対比例に係る推定値Cγmagも真値に近い値と看做されるため、対比例に係る状態ベクトルCxkが、観測残差によって、真値に近づくように補正されることは無い。すなわち、対比例にかかる非線形カルマンフィルタKFで推定された地磁気の強さr及び伏角θは、真値とは大きく異なる偽値であるが、観測残差による更新によっても補正されない局所最適解であるため、その後、正しい値に収束(復元)することは無い。 Therefore, when the estimated value γ mag according to the present embodiment is a value close to the true value, the estimated value C γ mag related to the proportionality is also considered to be a value close to the true value, and therefore, The state vector C x k is not corrected so as to approach the true value by the observation residual. That is, the geomagnetic strength r and the dip angle θ estimated by the nonlinear Kalman filter KF, which is proportional to each other, are false values that are significantly different from the true values, but are locally optimal solutions that are not corrected even by updating due to observation residuals. After that, there is no convergence (restoration) to the correct value.
このように、本実施形態では、非線形カルマンフィルタKFの推定対象となる動的システムの状態を表す複数の物理量のうち、値の範囲を定めることのできる物理量については、定められた範囲以外の値とならないように、状態変数を定めた。
具体的には、非線形カルマンフィルタKFの推定対象となる地磁気の強さr及び伏角θを、変数ρを用いた関数exp(ρ)及び変数ηを用いた関数tan−1ηとして表すことで、地磁気の強さrがr>0という条件から逸脱せず、また、地磁気の伏角θが−π/2<θ<π/2という条件から逸脱しないようにすることを可能とした。これにより、状態ベクトルxkが局所最適解に陥ることを防止し、状態ベクトルxkが真値とはかけ離れた値で安定することを防止することが可能となった。
As described above, in the present embodiment, among the physical quantities that represent the state of the dynamic system that is the estimation target of the nonlinear Kalman filter KF, the physical quantities that can determine the range of values are values other than the determined range. State variables were defined so as not to become.
Specifically, the geomagnetism strength r and the dip angle θ to be estimated by the nonlinear Kalman filter KF are expressed as a function exp (ρ) using the variable ρ and a function tan −1 η using the variable η. The strength r of the magnetic field does not deviate from the condition that r> 0, and the dip angle θ of the geomagnetism does not deviate from the condition of −π / 2 <θ <π / 2. Thus, the state vector x k can be prevented from falling into a local optimal solution, the state vector x k becomes possible to prevent the stabilized at a value far from the true value.
<B.変形例>
本発明は上述した実施形態に限定されるものではなく、例えば、以下の変形が可能である。また、以下に示す変形例のうちの2以上の変形例を組み合わせることもできる。
<B. Modification>
The present invention is not limited to the above-described embodiments, and for example, the following modifications are possible. Also, two or more of the modifications shown below can be combined.
(1)変形例1
上述した実施形態では、非線形カルマンフィルタの一種であるシグマポイントカルマンフィルタを一例として説明したが、本発明はこれに限定されるのではなく、拡張カルマンフィルタなど、どのような非線形カルマンフィルタに適用してもよい。動的システムの状態を表す物理量のうち、値の範囲を定めることのできる物理量について、当該物理量が当該範囲から外れた値に変化しないような状態変数を定めることで、非線形カルマンフィルタを用いた演算において、状態ベクトルが局所最適解に陥ることを防止することが可能となる。
(1)
In the embodiment described above, a sigma point Kalman filter, which is a kind of nonlinear Kalman filter, has been described as an example. However, the present invention is not limited to this, and may be applied to any nonlinear Kalman filter such as an extended Kalman filter. For physical quantities that can determine the range of values among the physical quantities that represent the state of the dynamic system, by defining state variables that do not change the physical quantities to values outside the range, in calculations using nonlinear Kalman filters Thus, it is possible to prevent the state vector from falling into the local optimum solution.
(2)変形例2
上述した実施形態では、動的システムの状態を表す複数の物理量のうち、地磁気の強さr及び伏角θの値の取り得る範囲を制限するように状態変数xkを定めたが、本発明はこれに限定されるものでは無く、動的システムの状態を表す物理量のうち、値の取り得る範囲を定めることのできる物理量については、当該範囲から外れた値とならないような状態変数を定めても良い。これにより、拡張カルマンフィルタに適用した場合であっても、状態ベクトルが局所最適解に陥ることを防止することが可能となる。
(2)
In the above-described embodiment, the state variable x k is defined so as to limit the range of the values of the geomagnetism strength r and the dip angle θ among the plurality of physical quantities representing the state of the dynamic system. However, the present invention is not limited to this, and among physical quantities that represent the state of a dynamic system, for a physical quantity that can determine the range of values that can be taken, a state variable that does not deviate from the range may be determined. good. As a result, even when applied to the extended Kalman filter, it is possible to prevent the state vector from falling into a local optimum solution.
(3)変形例3
本発明では、非線形カルマンフィルタKFを用いて推定する動的システムの状態を表す物理量を、姿勢を表すクォータニオンq、地磁気の強さr、地磁気の伏角θ、角速度ω、角速度センサのオフセット推定値go、及び地磁気センサのオフセット推定値moとしたが、本発明はこれに限定されるものでは無く、これらのうちの一部について推定するものであっても良い。
(3)
In the present invention, the physical quantity representing the state of a dynamic system that estimates using a non-linear Kalman filter KF, quaternion q, geomagnetic strength r, geomagnetic dip θ representing the orientation, the angular velocity omega, the angular velocity sensor offset estimate g o , and was offset estimate m o of the geomagnetic sensor, the present invention is not limited to this and may be one that estimated for some of these.
(4)変形例4
本発明では、観測値ベクトルykを、地磁気センサの出力である観測値m、加速度センサの出力である観測値a、及び角速度センサの出力である観測値gを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、このうちの一部のみを利用して観測値ベクトルを生成しても良い。
(4) Modification 4
In the present invention, the observed value vector y k is a vector whose elements are the observed value m that is the output of the geomagnetic sensor, the observed value a that is the output of the acceleration sensor, and the observed value g that is the output of the angular velocity sensor. The present invention is not limited to this, and the observation value vector may be generated using only a part of them.
1…携帯電話機、70…地磁気検出部、80…加速度検出部、90…角速度検出部、KF…非線形カルマンフィルタ、100…地磁気測定プログラム、110…初期値生成モジュール、120…非線形カルマンフィルタモジュール、210…シグマポイント生成部、220…状態遷移モデル部、230…平均算出部、240…観測モデル部、250…平均処理部、270…カルマンゲイン付与部。
DESCRIPTION OF
Claims (4)
状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する動作を実行するカルマンフィルタと、を備え、
第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、
第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、
前記第1変数と前記第2変数とが、
前記状態ベクトルの要素であることを特徴とする地磁気測定装置。 A plurality of sensors for observing the system and outputting observation values,
A state vector having a plurality of state variables indicating the state of the system as an element is estimated using a state transition model, an estimated observation value is calculated from the state vector using an observation value model, and the estimated observation value and the plurality of A Kalman filter that calculates a difference between the observed value of the sensor as an observation residual and executes an operation of updating the state vector based on the observation residual and the state vector,
A function that represents the strength of geomagnetism using the first variable and takes a value larger than the first value is defined as a first function.
Using the second variable to represent the dip angle of the geomagnetism, when the function that takes a value larger than the second value and smaller than the third value is the second function,
The first variable and the second variable are:
A geomagnetism measuring apparatus which is an element of the state vector.
前記第1関数は、exp(ρ)で表され、
前記第2関数は、tan−1ηで表されることを特徴とする、
請求項1に記載の地磁気測定装置。 When the first variable is ρ and the second variable is η,
The first function is represented by exp (ρ),
The second function is represented by tan −1 η.
The geomagnetic measurement apparatus according to claim 1.
状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定し、
観測値モデルを用いて前記状態ベクトルから推定観測値を算出し、
前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出し、
前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新し、
第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする地磁気測定方法。 A geomagnetism measurement method used in a geomagnetism measurement device including a plurality of sensors that observe a system and output observation values, respectively.
Using a state transition model to estimate a state vector whose elements are a plurality of state variables indicating the state of the system;
Calculate the estimated observation value from the state vector using the observation value model,
The difference between the estimated observation value and the observation value of the plurality of sensors is calculated as an observation residual,
Updating the state vector based on the observation residual and the state vector;
The first variable is used to represent the strength of the geomagnetism, and the function that takes a value larger than the first value is the first function, and the second variable is used to represent the dip of geomagnetism. Wherein the first variable and the second variable are elements of the state vector, when the function having a larger value and smaller than the third value is a second function. .
状態遷移モデルを用いて前記システムの状態を示す複数の状態変数を要素とする状態ベクトルを推定する処理と、
観測値モデルを用いて前記状態ベクトルから推定観測値を算出する処理と、
前記推定観測値と前記複数のセンサの観測値との差分を観測残差として算出する処理と、
前記観測残差と前記状態ベクトルとに基づいて前記状態ベクトルを更新する処理とを、コンピュータに実行させ、
第1変数を用いて、地磁気の強さを表し、第1の値よりも大きな値をとる関数を、第1関数とし、第2変数を用いて、地磁気の伏角を表し、第2の値よりも大きく且つ第3の値よりも小さな値をとる関数を、第2関数としたとき、前記第1変数と前記第2変数とが、前記状態ベクトルの要素であることを特徴とする地磁気測定プログラム。
A geomagnetism measurement program used in a geomagnetism measurement apparatus including a plurality of sensors that observe a system and output observation values, respectively.
Processing for estimating a state vector having a plurality of state variables indicating the state of the system using a state transition model;
A process of calculating an estimated observation value from the state vector using an observation value model;
A process of calculating a difference between the estimated observation value and the observation values of the plurality of sensors as an observation residual;
Processing to update the state vector based on the observation residual and the state vector;
The first variable is used to represent the strength of the geomagnetism, and the function that takes a value larger than the first value is the first function, and the second variable is used to represent the dip of geomagnetism. Wherein the first variable and the second variable are elements of the state vector, where the second function is a function that is larger and smaller than the third value. .
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011108125A JP2012237704A (en) | 2011-05-13 | 2011-05-13 | Terrestrial magnetism measuring device, terrestrial magnetism measuring method and terrestrial magnetism measurement program |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011108125A JP2012237704A (en) | 2011-05-13 | 2011-05-13 | Terrestrial magnetism measuring device, terrestrial magnetism measuring method and terrestrial magnetism measurement program |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2012237704A true JP2012237704A (en) | 2012-12-06 |
Family
ID=47460708
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011108125A Withdrawn JP2012237704A (en) | 2011-05-13 | 2011-05-13 | Terrestrial magnetism measuring device, terrestrial magnetism measuring method and terrestrial magnetism measurement program |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2012237704A (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113160299A (en) * | 2021-01-28 | 2021-07-23 | 西安电子科技大学 | Vehicle video speed measurement method based on Kalman filtering and computer readable storage medium |
-
2011
- 2011-05-13 JP JP2011108125A patent/JP2012237704A/en not_active Withdrawn
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113160299A (en) * | 2021-01-28 | 2021-07-23 | 西安电子科技大学 | Vehicle video speed measurement method based on Kalman filtering and computer readable storage medium |
CN113160299B (en) * | 2021-01-28 | 2023-04-18 | 西安电子科技大学 | Vehicle video speed measurement method based on Kalman filtering and computer readable storage medium |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Kok et al. | Calibration of a magnetometer in combination with inertial sensors | |
Michel et al. | A comparative analysis of attitude estimation for pedestrian navigation with smartphones | |
US8645093B2 (en) | Calibrating multi-dimensional sensor for offset, sensitivity, and non-orthogonality | |
US20140122015A1 (en) | Attitude estimation method and apparatus | |
Michel et al. | Attitude estimation for indoor navigation and augmented reality with smartphones | |
JP6705972B2 (en) | Attitude estimation device, attitude estimation method, control program, and recording medium | |
CN107785663A (en) | antenna beam attitude control method and system | |
JP7025215B2 (en) | Positioning system and positioning method | |
KR101211703B1 (en) | Calibration method of the magnetometer error using a line of sight vector and the integrated navigation system using the same | |
KR20140025319A (en) | Apparatuses and methods for dynamic tracking and compensation of magnetic near field | |
Wahdan et al. | Three-dimensional magnetometer calibration with small space coverage for pedestrians | |
CN106813679B (en) | Method and device for estimating attitude of moving object | |
JP2017166895A (en) | Electronic apparatus, sensor calibration method, and sensor calibration program | |
Li et al. | Common frame based unscented quaternion estimator for inertial-integrated navigation | |
JP2013096724A (en) | State estimation device | |
JP2013122384A (en) | Kalman filter and state estimation device | |
JP2013061309A (en) | Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter | |
Zhong et al. | PMI-Based Nonlinear $ H_\infty $ Estimation of Unknown Sensor Error for INS/GPS Integrated System | |
Allotta et al. | Underwater vehicles attitude estimation in presence of magnetic disturbances | |
JP2014219340A (en) | Offset correction method and offset correction device | |
JP6384194B2 (en) | Information processing apparatus, information processing method, and information processing program | |
Spielvogel et al. | Preliminary results with a low-cost fiber-optic gyrocompass system | |
US10648812B2 (en) | Method for filtering the signals arising from a sensor assembly comprising at least one sensor for measuring a vector physical field which is substantially constant over time and in space in a reference frame | |
JP2006038650A (en) | Posture measuring method, posture controller, azimuth meter and computer program | |
JP2012237704A (en) | Terrestrial magnetism measuring device, terrestrial magnetism measuring method and terrestrial magnetism measurement program |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A300 | Withdrawal of application because of no request for examination |
Free format text: JAPANESE INTERMEDIATE CODE: A300 Effective date: 20140805 |