JP2013061309A - Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter - Google Patents
Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter Download PDFInfo
- Publication number
- JP2013061309A JP2013061309A JP2011201544A JP2011201544A JP2013061309A JP 2013061309 A JP2013061309 A JP 2013061309A JP 2011201544 A JP2011201544 A JP 2011201544A JP 2011201544 A JP2011201544 A JP 2011201544A JP 2013061309 A JP2013061309 A JP 2013061309A
- Authority
- JP
- Japan
- Prior art keywords
- vector
- estimated
- state
- covariance
- observation
- 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)
Abstract
Description
本発明は、カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラムに関する。 The present invention relates to a Kalman filter, a state estimation device, a Kalman filter control method, and a Kalman filter control program.
近年、携帯機器の高性能化及び多用途化に伴い、地磁気の方向や携帯機器の姿勢等の推定機能を備える携帯機器の研究開発が盛んに行われている。地磁気の方向や携帯機器の姿勢等を推定する場合、地磁気センサ等の単体のセンサからの出力のみを利用して推定するよりも、異種の物理量を測定する複数のセンサからの出力を統合して推定する方が、より高速に正確な値を推定することができる。 2. Description of the Related Art In recent years, research and development of mobile devices having functions for estimating the direction of geomagnetism, the posture of mobile devices, and the like have been actively conducted with the improvement in performance and versatility of mobile devices. When estimating the direction of geomagnetism or the posture of a mobile device, the output from multiple sensors that measure different physical quantities is integrated, rather than using only the output from a single sensor such as a geomagnetic sensor. By estimating, an accurate value can be estimated at a higher speed.
異種の物理量を測定する複数のセンサの出力を統合し、動的システムの状態を推定する方法として、拡張カルマンフィルタ(EKF)やシグマポイントカルマンフィルタ(SPKF)等の非線形カルマンフィルタを用いる方法が知られている。
例えば、特許文献1には、3軸の角速度センサ及び3軸の加速度センサと非線形カルマンフィルタとを実装した姿勢角計測装置が開示されている。また、非特許文献1には、3軸角速度センサ、3軸加速度センサ、及び3軸地磁気センサからの出力信号を、アンセンテッド変換を用いたシグマポイントカルマンフィルタによって統合し、姿勢を推定する方法が開示されている。シグマポイントカルマンフィルタは、他のカルマンフィルタに比べて、非線形システムのモデル化に適した演算方法であり、シグマポイントカルマンフィルタを用いることで、非線形な動的システムの状態を正確に推定することができる。
As a method for estimating the state of a dynamic system by integrating the outputs of a plurality of sensors that measure different physical quantities, a method using a nonlinear Kalman filter such as an extended Kalman filter (EKF) or a sigma point Kalman filter (SPKF) is known. .
For example,
しかし、シグマポイントカルマンフィルタによる演算は計算量が多いため、シグマポイントカルマンフィルタが実装される機器には、大容量のメモリと高性能のCPUとを備えることが必要となる。そして、シグマポイントカルマンフィルタによる演算を行う場合、多くの電力が消費される。特に、シグマポイントカルマンフィルタを実装した機器が、バッテリーで給電される携帯機器の場合、機器の長時間使用が困難になるという課題が存在した。
本発明は、上述した点に鑑みてなされたものであり、シグマポイントカルマンフィルタの演算を簡素化し、高速且つ低消費電力な状態推定装置の実現を解決課題とする。
However, since the computation by the sigma point Kalman filter is computationally intensive, a device on which the sigma point Kalman filter is mounted needs to include a large-capacity memory and a high-performance CPU. And when calculating by a sigma point Kalman filter, much electric power is consumed. In particular, when a device equipped with a sigma point Kalman filter is a portable device powered by a battery, there is a problem that it is difficult to use the device for a long time.
The present invention has been made in view of the above-described points, and an object of the present invention is to realize a state estimation device that simplifies the calculation of the sigma point Kalman filter and achieves high speed and low power consumption.
以下、本発明について説明する。なお、本発明の理解を容易にするために本実施形態、変形例、及び添付図面の参照符号を括弧書きにて付記するが、それにより本発明が本実施形態に限定されるものではない。 The present invention will be described below. In addition, in order to make an understanding of this invention easy, although this embodiment, a modified example, and the reference sign of an accompanying drawing are attached in parentheses, this invention is not limited to this embodiment by it.
上記課題を解決するため、本発明に係るカルマンフィルタは、第1ベクトル(β+ k−1)及び第2ベクトル(q+ k−1)を要素とする状態ベクトル(x+ k−1)と、前記状態ベクトルの推定誤差の共分散(P+ k−1)と、に基づいて、状態遷移モデル(f)を用いて、推定第1ベクトル(β− k)及び推定第2ベクトル(q− k)を要素とする推定状態ベクトル(x− k)を算出する推定状態ベクトル算出部と、前記推定状態ベクトルの推定誤差の共分散(P− k)を算出する共分散算出部と、前記状態遷移モデル及び観測モデル(h)を用いて、前記状態ベクトルから、推定観測値ベクトル(y− k)を算出する推定観測値ベクトル算出部と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトル(yk)とに基づいて、観測残差(ek)を算出する観測残差生成部と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、前記推定状態ベクトル算出部は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分(P+ ββ,k−1)、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分(Qββ)を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分(P− ββ,k)を算出する、ことを特徴とする。 In order to solve the above-described problem, a Kalman filter according to the present invention includes a state vector (x + k-1 ) having a first vector (β + k-1 ) and a second vector (q + k-1 ) as elements, Based on the state vector estimation error covariance (P + k−1 ), the state transition model (f) is used to estimate the first vector (β − k ) and the second estimated vector (q − k). ) As an element, an estimated state vector calculation unit that calculates an estimated state vector (x − k ), a covariance calculation unit that calculates a covariance (P − k ) of an estimation error of the estimated state vector, and the state transition Using the model and the observation model (h), an estimated observation value vector calculation unit that calculates an estimated observation value vector (y − k ) from the state vector, the estimated observation value vector, and outputs from a plurality of external sensors Observed Based on an observation value vector (y k ) having elements as values, an observation residual generation unit that calculates an observation residual (e k ), the observation residual, and the estimated state vector, An update unit that calculates a vector obtained by updating a state vector, the estimated state vector calculation unit sets the estimated first vector to a value equal to the first vector, and the covariance calculation unit includes the Of the covariance of the state vector estimation error, a component (P + ββ, k−1 ) representing the covariance of the estimation error of the first vector, and the process noise covariance generated in the state vector in the state transition model among the components representing the covariance of the process noise generated in the first vector (Q ββ) by adding, among the covariance of the estimation error of the estimated state vector, the estimation error of the estimated first vector Components representing the covariance (P - ββ, k) is calculated, it is characterized.
この発明によれば、共分散算出部は、推定状態ベクトルの推定誤差の共分散のうち推定第1ベクトルの推定誤差の共分散を表す成分を、2個の行列を加算することで算出する。
一般的に、あるベクトルの推定誤差の共分散は、まず、当該あるベクトルの推定誤差を表すベクトルを算出し、その後、推定誤差を表すベクトルと、推定誤差を表すベクトルを転置したベクトルとの積を算出することにより求められる。よって、あるベクトルの推定誤差の共分散を求めるためには、推定誤差を表すベクトルを算出する演算の他に、当該あるベクトルの次元を表す数の二乗に相当する回数の演算が必要になる。
シグマポイントカルマンフィルタは、複数のシグマポイントに基づいて、推定状態ベクトルの推定誤差を表すベクトルを複数生成し、生成した複数の推定誤差を表すベクトルを用いて、推定状態ベクトルの推定誤差の共分散を算出する。従って、推定状態ベクトルの推定誤差の共分散を求めるためには、推定状態ベクトルの次元を表す数の二乗に相当する回数の演算を、更に、シグマポイントの個数に相当する回数だけ繰り返すことが必要となる。このように、推定状態ベクトルの推定誤差の共分散を求める演算は、計算量が多く、シグマポイントカルマンフィルタの処理負荷を増大させる要因となっていた。
これに対して、この発明の共分散算出部は、推定状態ベクトルの推定誤差の共分散のうちの一部の成分を、2つの行列の加算という単純な演算により算出するため、従来の方法に従って計算した場合に比べて、推定状態ベクトルの推定誤差の共分散を求める演算に係る計算量の大幅な低減を可能とした。これにより、シグマポイントカルマンフィルタの演算の高速化と、シグマポイントカルマンフィルタの演算に必要な電力の低減が可能となった。
なお、この発明の推定状態ベクトル算出部は、推定第1ベクトルを第1ベクトルと等しい値に設定する。しかし、第1ベクトルは、カルマンフィルタの演算において、値を全く変化させない訳ではない。すなわち、推定第1ベクトルを要素として含む推定状態ベクトルは、更新部において観測残差による更新がなされ、カルマンフィルタの推定対象であるシステムの状態を正確に表す値(真値)へと更新される。これにより、この発明に係るカルマンフィルタは、システムの状態を正確に推定することができる。
According to this invention, the covariance calculation unit calculates a component representing the covariance of the estimation error of the estimated first vector among the covariances of the estimation error of the estimated state vector by adding the two matrices.
In general, the covariance of an estimation error of a vector is calculated by first calculating a vector representing the estimation error of the vector and then multiplying the vector representing the estimation error by a vector obtained by transposing the vector representing the estimation error. It is calculated | required by calculating. Therefore, in order to obtain the covariance of the estimation error of a certain vector, in addition to the calculation for calculating the vector representing the estimation error, the calculation corresponding to the square of the number representing the dimension of the certain vector is required.
The sigma point Kalman filter generates a plurality of vectors representing the estimation error of the estimated state vector based on the plurality of sigma points, and uses the generated vector representing the estimation error to calculate the covariance of the estimation error of the estimated state vector. calculate. Therefore, in order to obtain the covariance of the estimation error of the estimated state vector, it is necessary to repeat the calculation corresponding to the square of the number representing the dimension of the estimated state vector and the number of times corresponding to the number of sigma points. It becomes. As described above, the calculation for obtaining the covariance of the estimation error of the estimated state vector has a large amount of calculation and has been a factor of increasing the processing load of the sigma point Kalman filter.
On the other hand, the covariance calculation unit according to the present invention calculates a part of the covariance of the estimation state vector estimation error by a simple operation of adding two matrices. Compared to the case of calculation, the calculation amount related to the calculation for obtaining the covariance of the estimation error of the estimated state vector can be greatly reduced. As a result, the calculation of the sigma point Kalman filter can be speeded up and the power required for the calculation of the sigma point Kalman filter can be reduced.
Note that the estimated state vector calculation unit of the present invention sets the estimated first vector to a value equal to the first vector. However, the first vector does not change the value at all in the calculation of the Kalman filter. That is, the estimated state vector including the estimated first vector as an element is updated by the observation residual in the updating unit, and updated to a value (true value) that accurately represents the state of the system that is the estimation target of the Kalman filter. Thereby, the Kalman filter according to the present invention can accurately estimate the state of the system.
また、上述したカルマンフィルタにおいて、前記推定状態ベクトル算出部は、前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイント(χ+ k−1)を生成するシグマポイント生成部と、前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイント(χ− k)を算出する状態遷移モデル部と、前記推定状態ベクトル(x− k)を算出する平均算出部と、を備え、前記状態遷移モデル部は、前記推定シグマポイントのうち、前記推定第2ベクトル(q− k)を算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第1ベクトル(β+ k−1)に基づいて生成された要素と等しい値に設定し、前記平均算出部は、前記推定第1ベクトル(β− k)を、前記第1ベクトル(β+ k−1)と等しい値に設定し、前記推定第2ベクトル(q− k)を、前記複数の推定シグマポイント(χ− k)に基づいて算出することが好ましい。 In the Kalman filter described above, the estimated state vector calculation unit generates a plurality of sigma points (χ + k−1 ) based on the state vector and the covariance of the state vector estimation error. Using the generation unit, the state transition model, a state transition model unit that calculates a plurality of estimated sigma points (χ − k ) from the plurality of sigma points, and the estimated state vector (x − k ) An average calculation unit, wherein the state transition model unit includes, among the estimated sigma points, elements other than the elements used for calculating the estimated second vector (q − k ) the first set equal to the generated elements on the basis of the vector (beta + k-1), the average calculating unit, the estimated first vector ( - a k), is set to the first vector (β + k-1) and equal, the estimated second vector (q - a k), said plurality of putative sigma points (chi - based on k) calculated It is preferable to do.
この発明によれば、状態ベクトルの推定値を、複数のシグマポイントを用いて算出する。これにより、状態遷移モデルを用いた状態ベクトルの推定において、状態ベクトルが有する誤差による影響と、カルマンフィルタが推定対象とするシステムの非線形性による影響とを低減させることができ、状態遷移モデルにおける推定精度の低下を防止することができる。すなわち、この発明によれば、カルマンフィルタが、システムの状態を正確に推定することが可能となる。
また、この発明によれば、推定第1ベクトルを、第1ベクトルと等しい値に設定し、推定第2ベクトルのみを、複数の推定シグマポイントに基づいて算出するため、推定状態ベクトルの算出に係る計算負荷を低減させることが可能となる。
According to the present invention, the estimated value of the state vector is calculated using a plurality of sigma points. As a result, in the estimation of the state vector using the state transition model, the influence due to the error of the state vector and the influence due to the nonlinearity of the system targeted by the Kalman filter can be reduced, and the estimation accuracy in the state transition model can be reduced. Can be prevented. That is, according to the present invention, the Kalman filter can accurately estimate the state of the system.
Further, according to the present invention, the estimated first vector is set to a value equal to the first vector, and only the estimated second vector is calculated based on a plurality of estimated sigma points. It is possible to reduce the calculation load.
次に、本発明に係る状態推定装置は、システムを観測して観測値を各々出力する複数のセンサと、上記のうちいずれかのカルマンフィルタとを備えることを特徴とする。
この発明によれば、高速なカルマンフィルタの演算が可能で、且つ低消費電力な状態推定装置が実現される。
Next, the state estimation apparatus according to the present invention includes a plurality of sensors that observe the system and output observation values, respectively, and any one of the Kalman filters.
According to the present invention, it is possible to realize a state estimation device capable of performing a high-speed Kalman filter and having low power consumption.
次に、本発明に係るカルマンフィルタの制御方法は、システムの状態を推定するカルマンフィルタの制御方法であって、第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する工程と、前記推定状態ベクトルの推定誤差の共分散を算出する工程と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、前記推定状態ベクトルを算出する工程は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。 Next, a Kalman filter control method according to the present invention is a Kalman filter control method for estimating a system state, in which a state vector having a first vector and a second vector as an element and an estimation error of the state vector are shared. And a step of calculating an estimated state vector having the estimated first vector and the estimated second vector as elements using a state transition model, and a step of calculating a covariance of the estimated error of the estimated state vector And using the state transition model and the observation model, a step of calculating an estimated observation value vector from the state vector, the estimated observation value vector, and observation values output from a plurality of external sensors as elements. A step of calculating an observation residual based on the observation value vector, the state residual based on the observation residual, and the estimated state vector. Calculating an estimated state vector, and the step of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector, and estimates an estimation error of the estimated state vector. The step of calculating the covariance includes a component representing the covariance of the estimation error of the first vector among the covariance of the estimation error of the state vector, and a covariance of process noise generated in the state vector in the state transition model A component representing the covariance of the process noise occurring in the first vector is added, and the component representing the covariance of the estimation error of the estimated first vector is calculated from the covariance of the estimation error of the estimated state vector. It is characterized by calculating.
この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。 According to the present invention, it is possible to significantly reduce the amount of calculation for calculating the covariance of the estimation error of the estimated state vector as compared with the case where the calculation is performed according to the conventional method. The power required for Kalman filter operation can be reduced.
次に、本発明に係るカルマンフィルタの制御プログラムは、システムの状態を推定するカルマンフィルタの制御プログラムであって、第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する処理と、前記推定状態ベクトルの推定誤差の共分散を算出する処理と、前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、前記推定状態ベクトルを算出する処理は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とする。 Next, a Kalman filter control program according to the present invention is a Kalman filter control program for estimating a system state, and a state vector having elements of a first vector and a second vector and a state vector estimation error are shared. And a process for calculating an estimated state vector having the estimated first vector and the estimated second vector as elements using a state transition model, and a process for calculating a covariance of the estimated error of the estimated state vector And using the state transition model and the observation model as an element, processing for calculating an estimated observation value vector from the state vector, the estimated observation value vector, and observation values output from a plurality of external sensors Based on the observation value vector, based on the processing for calculating the observation residual, the observation residual, and the estimated state vector A process of calculating a vector obtained by updating the state vector, and the process of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector, and the estimated state The process of calculating the covariance of the vector estimation error occurs in the state vector in the state transition model and the component representing the covariance of the first vector estimation error in the state vector estimation error covariance. Of the process noise covariance, a component representing the process noise covariance generated in the first vector is added, and the estimation error covariance of the estimated first vector out of the estimated error covariance of the estimated state vector is added. A component representing dispersion is calculated.
この発明によれば、推定状態ベクトルの推定誤差の共分散を求める演算の計算量を、従来の方法に従って計算した場合に比べて大幅に低減することが可能となり、カルマンフィルタの演算の高速化と、カルマンフィルタの演算に必要な電力の低減が可能となった。 According to the present invention, it is possible to significantly reduce the amount of calculation for calculating the covariance of the estimation error of the estimated state vector as compared with the case where the calculation is performed according to the conventional method. The power required for Kalman filter operation can be reduced.
<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 portable device according to an embodiment of the present invention, and FIG. 2 is a perspective view showing an appearance of the portable device. The
携帯機器1は、各種の構成要素とバスを介して接続され装置全体を制御するCPU10、CPU10の作業領域として機能するRAM20、各種のプログラムやデータを記憶したROM30、通信を実行する通信部40、画像を表示する表示部50、及びGPS部60を備える。GPS部60は、GPS衛星からの信号を受信して携帯機器1の位置情報(緯度、経度)を生成するものである。また、携帯機器1は、地磁気を検出して磁気データを出力する3次元磁気センサ70、加速度を検出して加速度データを出力する3次元加速度センサ80、及び角速度を検出して角速度データを出力する3次元角速度センサ90を備える。
The
3次元磁気センサ70は、X軸磁気センサ71、Y軸磁気センサ72、及びZ軸磁気センサ73を備える。各センサは、MI素子(磁気インピーダンス素子)、MR素子(磁気抵抗効果素子)などを用いて構成することができる。磁気センサI/F74は、各センサからの出力信号をAD変換して磁気データmを出力する。この磁気データmは、x軸、y軸およびz軸の3成分によって表されるベクトルデータである。
The three-dimensional
ところで、3次元磁気センサ70が搭載される携帯機器1は、着磁性を有する各種金属や、電気回路等、磁界を発生させる部品が備えられることが多い。このため、3次元磁気センサ70が出力する磁気データmは、磁極北に向かう水平成分と伏角方向の鉛直成分とを有する地磁気を表すベクトルの他に、機器に搭載された部品が発する磁界等を表すベクトルも含む値となる。従って、地磁気の値を正確に知るためには、3次元磁気センサが出力するベクトルデータ(磁気データm)から、携帯機器1の部品が発する内部磁界を表すベクトルを取り除く補正処理が必要となる。
このように、検出対象である地磁気の正確な値を得るために、補正処理において、3次元磁気センサ70から出力されるデータから取り除かれるべき内部磁界の値を3次元磁気センサ70のオフセットmOFFと呼ぶ。
なお、地磁気検出部70は、携帯機器1の外部にある外部物が発する磁界についても検出する。しかし、本実施形態では、外部物が発する磁界は無視できる程小さいものとする。
By the way, the
As described above, in order to obtain an accurate value of the geomagnetism that is a detection target, the value of the internal magnetic field to be removed from the data output from the three-dimensional
The
3次元加速度センサ80は、X軸加速度センサ81、Y軸加速度センサ82、及びZ軸加速度センサ83を備える。各センサは、ピエゾ抵抗型、静電容量型、熱検知型などどのような検知方式であってもよい。加速度センサI/F84は、各センサからの出力信号をAD変換して加速度データaを出力する。この加速度データaは、携帯機器1に固定された座標系における慣性力と重力との合力を、x軸、y軸及びz軸の3成分を有するベクトルとして示すデータである。したがって、携帯機器1が静止状態または等速直線運動状態にあれば、加速度データaは携帯機器1に固定された座標系において重力加速度の大きさと方向とを示すベクトルデータとなる。
The three-
3次元角速度センサ90は、X軸角速度センサ91、Y軸角速度センサ92、及びZ軸角速度センサ93を備える。角速度センサI/F94は、各センサからの出力信号をAD変換して角速度データgを出力する。角速度データgは、各軸の回りの角速度を示す3次元のベクトルデータである。なお、角速度データgには、3次元角速度センサ90のオフセットgOFFが重畳する。
The three-dimensional
CPU10は、ROM30に格納されている状態推定プログラム100を実行することによって、動的システムの状態を表す複数の物理量、例えば、携帯機器1の姿勢、3次元磁気センサ70のオフセット、地磁気の強さや伏角等の物理量を推定する。すなわち、携帯機器1は、CPU10が状態推定プログラム100を実行することにより、状態推定装置として機能する。
The
状態推定プログラム100は、初期値生成モジュール110と、カルマンフィルタモジュール120とを備える。カルマンフィルタモジュール120は、カルマンフィルタKFの演算を実行する。
The
[2. カルマンフィルタの観測対象及び推定対象]
一般的に、カルマンフィルタは、動的システムの状態を表す複数の物理量の経時的な変化を推定する状態遷移モデルと、動的システムを観測する複数のセンサが計測する値(観測値)を推定する観測モデルと、を有する。そして、カルマンフィルタは、状態遷移モデルを用いて、ある時刻における動的システムの状態を表す複数の物理量(状態変数)を要素とする状態ベクトルから、単位時間が経過した後の状態ベクトルを推定する。次に、カルマンフィルタは、推定された状態ベクトルに基づいて、動的システムの状態を測定する複数のセンサの出力値を要素とする観測値ベクトルの値を推定する。さらに、推定された観測値ベクトルと、実際のセンサの出力値を要素とする観測値ベクトルとの差分として算出される観測残差に基づいて、推定された状態ベクトルを、実際の値(真値)に近い値へと更新し、更新後の状態ベクトルを出力する。
カルマンフィルタは、以上のような演算を繰り返すことにより、状態ベクトルを構成する複数の状態変数の各々を、実際の物理量を正確に表した値(真値)へと近付ける。
[2. Observation target and estimation target of Kalman filter]
In general, the Kalman filter estimates a state transition model that estimates changes over time of a plurality of physical quantities representing the state of a dynamic system and values (observed values) measured by a plurality of sensors that observe the dynamic system. An observation model. Then, the Kalman filter uses a state transition model to estimate a state vector after a unit time has elapsed from a state vector whose elements are a plurality of physical quantities (state variables) representing the state of the dynamic system at a certain time. Next, based on the estimated state vector, the Kalman filter estimates the value of an observation value vector whose elements are output values of a plurality of sensors that measure the state of the dynamic system. Furthermore, based on the observation residual calculated as the difference between the estimated observation vector and the observation vector whose element is the actual sensor output value, the estimated state vector is converted to the actual value (true value). To a value close to) and output the updated state vector.
The Kalman filter repeats the above operation to bring each of a plurality of state variables constituting the state vector close to a value (true value) that accurately represents the actual physical quantity.
本実施形態において、カルマンフィルタKFは、携帯機器1の姿勢q、地磁気の強さr、地磁気の伏角θ、携帯機器1の角速度ω、3次元角速度センサ90のオフセットgOFFの推定値gO、及び3次元磁気センサ70のオフセットmOFFの推定値mOを状態変数として採用し、これらを推定の対象とする。
これらの状態変数を要素とする時刻T=kにおける状態ベクトルxkは、以下の式(1)で表される。なお、各状態変数の右下に付された添え字「k」は、当該状態変数が時刻T=kにおける値であることを表す。
A state vector x k at time T = k having these state variables as elements is expressed by the following equation (1). The subscript “k” attached to the lower right of each state variable indicates that the state variable is a value at time T = k.
本実施形態では、姿勢qを、クォータニオンを用いて表現する。クォータニオンとは、物体の姿勢(回転状態)を表す4次元数である。具体的には、地上に固定された座標系(例えば、地上の任意の一点を原点とし、互いに直交する3つの方向、例えば、水平東、水平北、及び鉛直上向きを、それぞれx軸、y軸、及びz軸とする座標系)を定め、当該地上に固定された座標系の各軸と、携帯機器1に固定された座標系の各軸とが一致する姿勢qを基準姿勢と定義し、基準姿勢をq=(0、0、0、1)と表現する。このとき、携帯機器1の任意の姿勢qは、基準姿勢に対して単位ベクトルρの方向を回転軸として角度ψだけ回転した姿勢として表現できる。回転後の姿勢qは、式(2)で与えられる。
本実施形態におけるカルマンフィルタの観測対象は、3次元磁気センサ70から出力される磁気データm、3次元加速度センサ80から出力される加速度データa、及び3次元角速度センサ90から出力される角速度データgである。観測値ベクトルは、これら磁気データm、加速度データa、及び角速度データgを要素とする。時刻T=kにおける観測値ベクトルykは式(3)で与えられる。
初期値生成モジュール110は、時刻T=0における状態変数を要素とする初期値INIを算出する。初期値INIは、姿勢qの初期値q0、地磁気の強さrの初期値r0、地磁気の伏角θの初期値θ0、角速度ωの初期値ω0、3次元角速度センサ90のオフセット推定値gOの初期値gO,0、及び3次元磁気センサ70のオフセット推定値mOの初期値mO,0を要素として含む。
初期値INIは、カルマンフィルタの演算によって状態変数がなるべく早く正確な値に収束するような値に適宜設定すればよいが、例えば、以下のような値に設定してもよい。
The initial
The initial value INI may be appropriately set to a value such that the state variable converges to an accurate value as soon as possible by calculation of the Kalman filter, but may be set to the following value, for example.
地磁気の強さrの初期値r0、及び地磁気の伏角θの初期値θ0は、例えば、GPS部60から供給される位置情報に基づいて生成することができる。これは、地球上の位置が特定できれば、その位置における地磁気の強さ及び伏角を知ることができるからである。具体的には、ROM30には、位置情報と地磁気の強さ及び伏角とを対応づけて記憶したルックアップテーブルLUT1が格納される。初期値生成モジュール110は、ルックアップテーブルLUT1を参照して、位置情報に対応する地磁気の強さ及び伏角を、地磁気の強さrの初期値r0及び地磁気の伏角θの初期値θ0として設定する。
なお、携帯機器1が衛星からの電波の届かない場所(例えば、地下街)にある場合には、GPS部60から位置情報が得られない。そのような場合には、携帯機器1が使われ得る地域の典型的な値を採用すればよい。その値もルックアップテーブルLUT1に格納されている。初期値生成モジュール110は、位置情報の取得が不能な場合には、典型的な値をルックアップテーブルLUT1から読み出す。例えば、日本で販売された携帯機器1については、明石市の地磁気の強さ及び伏角に基づいて、地磁気の強さrの初期値r0、及び地磁気の伏角θの初期値θ0を算出する。
The initial value r 0 of the geomagnetic strength r and the initial value θ 0 of the geomagnetic depression angle θ can be generated based on, for example, position information supplied from the
In addition, when the
角速度ωの初期値ω0は、例えば、携帯機器1が静止していることを仮定して、「0」に設定する。3次元角速度センサ90のオフセット推定値gOの初期値gO,0は、通常「0」近辺に調整されているため、「0」に設定する。姿勢qの初期値q0に関しては、例えば、携帯機器1が一定方向に向いて静止していることを仮定して、実際の初期姿勢とのずれを小さくするような値を設定する。
The initial value ω 0 of the angular velocity ω is set to “0”, for example, assuming that the
3次元磁気センサ70のオフセット推定値mOの初期値mO,0は、例えば、時刻T=0の3次元磁気センサ70の観測値m0、姿勢qの初期値q0、携帯機器1を使用する地域の地磁気ベクトルmtypical、及び、式(5)に示す行列B(q)を用いて、以下に示す式(4)で与えられる値に設定する。ここで、行列B(q)は、携帯機器1が姿勢qである場合に、地上に固定された座標系において表現されたベクトルを、携帯機器1に固定された座標系において表現するための座標変換行列である。なお、ROM30には、位置情報と地磁気ベクトルmtypicalとを対応づけて記憶したルックアップテーブルLUT2が記憶されている。初期値生成モジュール110は、GPS部60で生成される位置情報に基づいてルックアップテーブルLUT2を参照して地磁気ベクトルmtypicalを取得し、式(4)を演算することによって、オフセット推定値mOの初期値mO,0を得る。
初期値生成モジュール110は、以上のようにして生成された姿勢qの初期値q0、地磁気の強さrの初期値r0、地磁気の伏角θの初期値θ0、角速度ωの初期値ω0、3次元角速度センサ90のオフセット推定値gOの初期値gO,0、及び3次元磁気センサ70のオフセット推定値mOの初期値mO,0を要素とするベクトルである初期値INIを生成し、これを出力する。
The initial
[3. カルマンフィルタによる演算]
次に、本実施形態に係るカルマンフィルタKFによる演算の概要について説明する。
カルマンフィルタKFは、動的システムの状態の経時的な変化を表す状態遷移モデルを用いて、ある時刻(時刻T=k−1)のシステムの状態を示す状態ベクトルxk−1から単位時間が経過した後(時刻T=k)の状態ベクトルxkを推定し、この推定値を、推定された状態ベクトル(推定状態ベクトル)x− kとして出力する。そして、カルマンフィルタKFは、各種センサからの出力を要素とする観測値ベクトルykと状態ベクトルxkとの関係を表す観測モデルを用いて、状態遷移モデルにより推定された状態ベクトルx− kに基づいて観測値ベクトルykを推定し、この推定値を、推定された観測値ベクトル(推定観測値ベクトル)y− kとして出力する。なお、本実施形態のカルマンフィルタKFは、非線形カルマンフィルタであるシグマポイントカルマンフィルタにより構成される。シグマポイントカルマンフィルタとは、状態ベクトルxk−1の周囲に複数のシグマポイントχk−1を設定し、これらの複数のシグマポイントχk−1を状態遷移モデルに適用することで、単位時間経過後のシグマポイントχ− kを推定し、シグマポイントの推定値(推定シグマポイント)χ− kの平均を算出することにより、推定された状態ベクトルx− kを求める演算である。従って、推定された観測値ベクトルy− kは、厳密には、推定された状態ベクトルx− kの周辺に存在する複数のシグマポイントの推定値χ− kに基づいて算出される。
その後、カルマンフィルタKFは、推定された観測値ベクトルy− kと、実際の観測値を要素とする観測値ベクトルykとの差分を観測残差ekとして算出する。また、カルマンフィルタKFは、推定された状態ベクトルx− kと、推定された観測値ベクトルy− kと、に基づいてカルマンゲインKkを算出する。そして、カルマンフィルタKFは、観測残差ekとカルマンゲインKkとを用いて推定された状態ベクトルx− kを更新し、更新後の状態ベクトル(状態ベクトルを更新したベクトル)x+ kを算出する。
以上のような状態ベクトルxkの更新を繰り返すカルマンフィルタKFの演算により、状態ベクトルxkを実際の物理量を正確に表した値(真値)に近い正確な値へと近付けることができる。
[3. Calculation using Kalman filter]
Next, an outline of calculation by the Kalman filter KF according to the present embodiment will be described.
The Kalman filter KF uses a state transition model that represents a change in the dynamic system state over time, and a unit time has elapsed from a state vector x k-1 indicating the system state at a certain time (time T = k-1). and it estimates the state vector x k after (time T = k) was, this estimate, the estimated state vector (estimated state vector) x - output as k. The Kalman filter KF is based on the state vector x − k estimated by the state transition model using an observation model representing the relationship between the observation value vector y k and the state vector x k whose elements are outputs from various sensors. The observed value vector y k is estimated, and this estimated value is output as an estimated observed value vector (estimated observed value vector) y − k . Note that the Kalman filter KF of the present embodiment is configured by a sigma point Kalman filter that is a nonlinear Kalman filter. The sigma point Kalman filter sets a plurality of sigma points χ k−1 around the state vector x k−1 , and applies these sigma points χ k−1 to the state transition model, so that a unit time elapses. This is an operation for estimating the subsequent sigma point χ − k and calculating the average of the estimated value (estimated sigma point) χ − k of the sigma point to obtain the estimated state vector x − k . Therefore, strictly speaking, the estimated observation value vector y − k is calculated based on the estimated values χ − k of a plurality of sigma points existing around the estimated state vector x − k .
Then, the Kalman filter KF is estimated observed value vector y - calculates and k, a difference between the observed value vector y k to the actual observed value element as observed residuals e k. The Kalman filter KF calculates the Kalman gain K k based on the estimated state vector x − k and the estimated observation value vector y − k . Then, the Kalman filter KF updates the state vector x − k estimated using the observation residual e k and the Kalman gain K k , and calculates the updated state vector (the vector obtained by updating the state vector) x + k . To do.
By calculating the Kalman filter KF that repeatedly updates the state vector x k as described above, the state vector x k can be brought close to an accurate value close to a value (true value) that accurately represents the actual physical quantity.
[3.1. カルマンフィルタ演算部の動作の概要]
本実施形態におけるカルマンフィルタKFの状態遷移モデルは以下に示す式(6)で与えられ、観測モデルは以下に示す式(7)で与えられる。本実施形態では、式(6)に現れる関数f及び式(7)に現れる関数hは、非線形の関数である。
The state transition model of the Kalman filter KF in this embodiment is given by the following equation (6), and the observation model is given by the following equation (7). In the present embodiment, the function f appearing in Expression (6) and the function h appearing in Expression (7) are nonlinear functions.
ここで、状態ベクトルxkはn次元のベクトルであり、観測値ベクトルykはm次元のベクトルである。なお、本実施形態では、n=15であり、m=9である。また、式(6)に現れるプロセスノイズuk及び式(7)に現れる観測ノイズvkは0を中心とするガウスノイズである。
式(6)は、時刻T=kにおける状態ベクトルxkが、時刻T=k−1における状態ベクトルxk−1を関数fに代入して得られた値と、時刻T=k−1におけるプロセスノイズuk−1とを加算することにより推定されるものであることを示している。
また、式(7)は、時刻T=kにおける観測値ベクトルykが、時刻T=kにおける状態ベクトルxkを関数hに代入して得られる値と、時刻T=kにおける観測ノイズvkとを加算することにより推定されることを示している。
なお、プロセスノイズukの共分散をQ、観測ノイズvkの共分散をRと表す。
Here, the state vector x k is an n-dimensional vector, and the observation value vector y k is an m-dimensional vector. In this embodiment, n = 15 and m = 9. Further, the process noise u k appearing in the equation (6) and the observation noise v k appearing in the equation (7) are Gaussian noises centered on zero.
Equation (6) shows that the state vector x k at time T = k is obtained by substituting the state vector x k−1 at time T = k−1 for the function f, and at time T = k−1. It shows that the noise is estimated by adding the process noise u k−1 .
Equation (7) shows that the observed value vector y k at time T = k is obtained by substituting the state vector x k at time T = k into the function h, and the observed noise v k at time T = k. It shows that it estimates by adding.
The covariance of the process noise u k is represented as Q, and the covariance of the observation noise v k is represented as R.
時刻T=kにおける観測残差ekは、実際の観測値ベクトルykと、推定された観測値ベクトルy− kとに基づいて定められるベクトルであり、以下に示す式(8)で表される。カルマンフィルタKFは、観測残差ekと、式(9)に示すカルマンゲインKkとを用いて、以下の式(10)に示すように、推定された状態ベクトルx− kを更新し、更新後の状態ベクトルx+ kを算出する。また、カルマンフィルタKFは、以下の式(11)に示すように、状態ベクトルxkの推定誤差の共分散Pkを更新する。
なお、P− kは、推定された状態ベクトルx− kの推定誤差の共分散であり、P+ kは、更新後の状態ベクトルx+ kの推定誤差の共分散である。
また、Pyy kは、観測残差ekの共分散であり、Pxy kは、推定された状態ベクトルx− kと、推定された観測値ベクトルy− kとの相互共分散行列である。
P - k is the covariance of the estimated error of the estimated state vector x - k , and P + k is the covariance of the estimated error of the updated state vector x + k .
Also, P yy k is the covariance of the observation residuals e k, P xy k is the state vector x is estimated - is the cross-covariance matrices between the k - and k, observed value vector y estimated .
観測モデルを用いて推定された観測値ベクトルy− kは、式(7)に示すように、観測モデルを用いて推定された状態ベクトルx− kに基づいて算出される理論値である。よって、観測モデルを用いて推定された観測値ベクトルy− kと実際のセンサ出力に基づく観測値ベクトルykとの差分である観測残差ekは、推定された状態ベクトルx− kと実際の物理量を正確に表した値(真値)との近似度を示す値である。
カルマンフィルタKFは、式(10)に示すように、観測残差ekを用いて、推定された状態ベクトルx− kを、真値に近い更新後の状態ベクトルx+ kへと更新する。
The observation value vector y − k estimated using the observation model is a theoretical value calculated based on the state vector x − k estimated using the observation model, as shown in Equation (7). Thus, the observed value vector y was estimated using the observation model - observation residuals e k is the difference between the observed value vector y k based on actual sensor output and k is estimated state vector x - actually a k This is a value indicating the degree of approximation with a value (true value) that accurately represents the physical quantity.
Kalman filter KF, as shown in equation (10), with the observed residual e k, estimated state vector x - a k, is updated to the state vector x + k after update close to the true value.
[3.2. シグマポイントカルマンフィルタによる演算]
本実施形態において、カルマンフィルタKFは、アンセンテッド変換を用いたシグマポイントカルマンフィルタにより構成される。以下では、図3に示すカルマンフィルタKFの機能ブロック図を参照しつつ、カルマンフィルタKFによる演算の詳細を説明する。
[3.2. Calculation with Sigma Point Kalman Filter]
In the present embodiment, the Kalman filter KF is configured by a sigma point Kalman filter using unscented transformation. Hereinafter, the details of the calculation by the Kalman filter KF will be described with reference to the functional block diagram of the Kalman filter KF shown in FIG.
遅延部121は、加算器132から出力される更新後の状態ベクトルx+ kを、単位時間(時刻T=k−1から時刻T=kに相当する時間)だけ遅延させることで、状態ベクトルx+ k−1を生成し、これを、シグマポイント生成部122に対して出力する。なお、初回の演算(時刻T=0)では、遅延部121は、初期値生成モジュール110が出力する初期値INIを用いて、状態ベクトルx+ k−1を生成する。
また、遅延部121は、減算器133から出力される更新後の状態ベクトルx+ kの推定誤差の共分散P+ kを単位時間遅延させることで、状態ベクトルx+ k−1の推定誤差の共分散P+ k−1を生成し、これを、シグマポイント生成部122及び共分散算出部125に対して出力する。
The
Further, the
シグマポイント生成部122は、状態ベクトルx+ k−1に基づいて、a個のシグマポイントχ+ k−1(i)(i=1,2,…a)を生成する(aは、2以上の自然数)。シグマポイントχ+ k−1(i)の生成は、以下に示す公知の方法を適用する。
シグマポイント生成部122は、まず、状態ベクトルx+ k−1の推定誤差の共分散P+ k−1から、式(12)に示す共分散P+ k−1の平方根を表すn行n列の行列を生成する。次に、シグマポイント生成部122は、共分散P+ k−1の平方根を表す行列と、各種係数とに基づいて、a個のシグマポイント算出用ベクトルを定める。シグマポイント算出用ベクトルは、n次元のベクトルである。その後、シグマポイント生成部122は、状態ベクトルx+ k−1と、a個のシグマポイント算出用ベクトルとに基づいて、a個のシグマポイントχ+ k−1(i)を生成し、これを状態遷移モデル部123に対して出力する。
なお、a個のシグマポイントχ+ k−1(i)の生成において用いられる各種係数は、後述する式(14)における重みwiの生成にも用いられる値であり、公知の方法により定められる。また、a個のシグマポイント算出用ベクトルには0ベクトルが含まれていても構わない。
Sigma
Note that various coefficients used in generating a sigma points χ + k−1 (i) are values used for generating weights w i in equation (14) described later, and are determined by a known method. . Also, the zero sigma point calculation vectors may include zero vectors.
状態遷移モデル部123は、式(13)に示すように、時刻T=k−1におけるa個のシグマポイントχ+ k−1(i)の各々を、状態遷移モデルに適用することにより、時刻T=kにおけるa個のシグマポイントの推定値χ− k(i)を算出する。
次に、平均算出部124は、式(14)に示すように、時刻T=kにおけるa個のシグマポイントの推定値χ− k(i)の重み付き平均を計算することで、推定された状態ベクトルx− kを算出し、これを出力する。なお、式(14)に現れる重みwiは、シグマポイントχ+ k−1(i)の生成において用いられた各種係数に基づいて定められる。
このように、シグマポイント生成部122、状態遷移モデル部123、及び平均算出部124は、状態ベクトルx+ k−1から、推定された状態ベクトルx− kを算出する推定状態ベクトル算出部140として機能する。
As described above, the sigma
共分散算出部125は、推定された状態ベクトルx− kの推定誤差の共分散P− kを算出する。推定された状態ベクトルx− kの推定誤差の共分散P− kは、式(15)に示すように、a個のシグマポイントの推定値χ− k(i)、推定された状態ベクトルx− k、重みwi、及びプロセスノイズuk−1の共分散Qに基づいて算出することができる。
但し、本実施形態では、処理負荷を削減する観点から、共分散P− kのうち一部の成分は、式(15)を用いずに、後述する式(30)に基づいて算出する。この点についての詳細は後述する。
However, in the present embodiment, from the viewpoint of reducing the processing load, some components of the covariance P − k are calculated based on Expression (30) described later without using Expression (15). Details of this point will be described later.
一方、観測モデル部126は、式(16)に示すように、時刻T=kにおけるa個のシグマポイントの推定値χ− k(i)の各々を、観測モデルに適用することにより、a個の推定観測値γk(i)を算出する。
そして、平均処理部127は、式(17)に示すように、a個の推定観測値γk(i)の平均を演算することにより、推定された観測値ベクトルy− kを算出する。
このように、シグマポイント生成部122、状態遷移モデル部123、観測モデル部126、及び平均処理部127は、状態ベクトルx+ k−1から、推定された観測値ベクトルy− kを算出する推定観測値ベクトル算出部150として機能する。
As described above, the sigma
次に、平均処理部127は、式(18)に示す観測残差の共分散Pyy kを算出する。
減算器(観測残差生成部)131は、式(8)に示したように、観測値ベクトルykと、推定された観測値ベクトルy− kとの差分として、観測残差ekを算出する。
カルマンゲイン付与部128は、式(19)に示す相互共分散行列Pxy kを算出する。次に、カルマンゲイン付与部128は、式(9)に示したように、観測残差の共分散Pyy kと、相互共分散行列Pxy kとに基づいて、カルマンゲインKkを算出する。その後、カルマンゲイン付与部128は、式(10)の右辺第2項(Kkek)を演算し、その結果を加算器132に対して出力する。
また、カルマンゲイン付与部128は、観測残差の共分散Pyy kとカルマンゲインKkとに基づいて、式(11)の右辺第2項(KkPyy kKT k)を演算し、その結果を減算器133に対して出力する。
The Kalman
Further, the Kalman gain assigning
加算器(更新部)132は、式(10)に示したように、推定された状態ベクトルx− kと、カルマンゲイン付与部128から出力される式(10)の右辺第2項(Kkek)と、を加算することにより、更新後の状態ベクトルx+ kを算出する。
減算器133は、式(11)に示したように、推定された状態ベクトルx− kの推定誤差の共分散P− kと、カルマンゲイン付与部128から出力される式(11)の右辺第2項(KkPyy kKT k)との差分として、更新後の状態ベクトルx+ kの推定誤差の共分散P+ kを算出する。
As shown in Expression (10), the adder (update unit) 132 outputs the estimated state vector x − k and the second term (K k ) on the right side of Expression (10) output from the Kalman gain assigning
[3.3. 状態遷移モデル]
次に、状態遷移モデル部123の演算で用いる状態遷移モデルについて説明する。
[3.3. State transition model]
Next, the state transition model used in the calculation of the state
本実施形態に係る状態遷移モデルにおいて、状態ベクトルxkを構成する状態変数のうち、姿勢qについての状態遷移は、式(21)に示す関数fq(q,ω)を用いて、式(20)により定義される。式(20)は、時刻T=kにおける推定された状態ベクトルx− kを構成する状態変数である姿勢q− kが、時刻T=k−1における状態ベクトルx+ k−1を構成する状態変数である姿勢q+ k−1及び角速度ω+ k−1に基づいて推定されることを示している。
式(21)に示す関数fq(q,ω)は、式(6)に示した関数fのうち、姿勢qを算出する部分のみを表した関数である。式(21)の右辺の演算子Ωは、式(22)により定義される。ここで、I3×3は3行3列の単位行列を表し、3次元ベクトルl=(l1,l2,l3)に対して、[l×]という記号を式(23)で定義する。また、測定時間間隔(時刻T=k−1から時刻T=kまでの間隔)をΔtで表したとき、演算子Ωを構成する成分Ψを、式(24)で定義する。
The function f q (q, ω) shown in Expression (21) is a function that represents only the part for calculating the posture q in the function f shown in Expression (6). The operator Ω on the right side of Equation (21) is defined by Equation (22). Here, I 3 × 3 represents a unit matrix of 3 rows and 3 columns, and for the three-dimensional vector l = (l 1 , l 2 , l 3 ), the symbol [l ×] is defined by Expression (23). To do. Further, when the measurement time interval (interval from time T = k−1 to time T = k) is represented by Δt, the component Ψ constituting the operator Ω is defined by Expression (24).
姿勢qは、クォータニオンで表現されるため、正規化条件||q||=1が満たされる必要がある。しかし、シグマポイントカルマンフィルタを用いて状態ベクトルxkを更新する場合、更新後の状態ベクトルx+ kは、式(14)に示すように、シグマポイントの推定値χ− k(i)に重みwiを付与した重み付き平均として算出されるため、(更新前の)状態ベクトルxk−1のノルムと、更新後の状態ベクトルx+ kのノルムとは、異なる値となることがある。従って、シグマポイントカルマンフィルタの演算を行う場合、(更新前の)状態ベクトルxk−1の要素である姿勢qk−1のノルムと、更新後の状態ベクトルx+ kの要素である姿勢qkのノルムとは、異なる値となる可能性が存在する。つまり、シグマポイントカルマンフィルタの演算を行う場合、姿勢qについての正規化条件が満たされなくなる可能性が存在する。
そこで、姿勢qに対して何らかの演算が行われるときには、演算後の結果をそのベクトル自身の大きさで正規化する。なお、より厳密に正規化条件を保つためには、状態ベクトルxkを構成する要素のうち姿勢qkについてはMRPs (modified Rodrigues parameters)を用いて前時刻との差分情報だけに限定し、カルマンフィルタの外部にある姿勢情報をカルマンフィルタから得られる差分情報に基づいて更新すればよい。
Since the posture q is expressed by a quaternion, the normalization condition || q || = 1 needs to be satisfied. However, when the state vector x k is updated using the sigma point Kalman filter, the updated state vector x + k is weighted to the estimated value χ − k (i) of the sigma point, as shown in Expression (14). Since it is calculated as a weighted average with i added, the norm of the state vector x k−1 (before update) and the norm of the state vector x + k after update may be different values. Therefore, when performing the calculation of sigma point Kalman filter, and the norm of the (pre-update) the state vector x k-1 is the element position q k-1, which is an element of the state vector x + k after update attitude q k There is a possibility of different values from the norm of. That is, when performing the sigma point Kalman filter calculation, there is a possibility that the normalization condition for the posture q is not satisfied.
Therefore, when any calculation is performed on the posture q, the result after the calculation is normalized with the size of the vector itself. In order to maintain the normalization condition more strictly, the posture q k among elements constituting the state vector x k is limited to only difference information from the previous time using MRPs (modified Rodrigues parameters), and the Kalman filter May be updated based on the difference information obtained from the Kalman filter.
地磁気の強さr及び地磁気の伏角θは、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおける地磁気の強さrk及び伏角θkと、時刻T=k−1における地磁気の強さrk−1及び伏角θk−1とは等しい値であるという前提を置く。
同様に、3次元角速度センサ90のオフセット推定値gOも、変化を予測することが難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおけるオフセット推定値gO,Kと、時刻T=k−1におけるオフセット推定値gO,K−1とは等しい値であるという前提を置く。
It is difficult to predict changes in the geomagnetic strength r and the geomagnetic dip angle θ. Therefore, in the state transition model of the present embodiment, for convenience, time T = and strength r k and dip theta k geomagnetic at k, the time T = geomagnetic strength in k-1 r k-1 and dip theta k The assumption is made that −1 is an equal value.
Similarly, it is difficult to predict a change in the estimated offset value g O of the three-dimensional
携帯機器1の角速度ωは、携帯機器1の利用者による携帯機器1の動かし方に依存して変化するため、時刻T=k−1の角速度ωk−1を用いて、時刻T=kにおける角速度ωkを定式化することは難しい。そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおける角速度ωkと、時刻T=k−1における角速度ωk−1とは等しいと仮定する。
Since the angular velocity ω of the
前述のとおり、3次元磁気センサ70のオフセットmOFFは、携帯機器1の部品が発する内部磁界である。従って、携帯機器1の内部状態が一定である間は、オフセットmOFFも変化しない。一方、携帯機器1の内部状態が変化した場合、例えば、携帯機器1に搭載された部品を流れる電流の大きさが変化した場合や、携帯機器1に搭載された部品の着磁状況が変化した場合には、オフセットmOFFも変化する。すなわち、携帯機器1の内部状態は、携帯機器1の利用者による携帯機器1の操作や、携帯機器1の外部の環境等に依存して変化する。従って、オフセットmOFFが変化するタイミングやオフセットmOFFの変化量を予測することは困難であり、時刻T=k−1におけるオフセット推定値mO,K−1を用いて、時刻T=kにおけるオフセット推定値mO,kを定式化することは難しい。
そこで、本実施形態に係る状態遷移モデルでは、便宜上、時刻T=kにおけるオフセット推定値mO,kと、時刻T=k−1におけるオフセット推定値mO,k−1とは等しいと仮定する。
As described above, the offset m OFF of the three-dimensional
Therefore, in the state transition model according to the present embodiment, for the sake of convenience, it is assumed that the estimated offset value m O, k at time T = k is equal to the estimated offset value m O, k−1 at time T = k−1. .
このように、本実施形態に係る状態遷移モデルでは、以下に示す式(25)のように、状態ベクトルxkを構成する状態変数のうち、姿勢q以外は、前の時刻から変化しないという前提を置く。
ここで、式(1)に示した状態ベクトルxkを、式(26)のように、姿勢qkと、姿勢qk以外の要素を表すベクトルβkとに分離して表現した場合、ベクトルβkについて、式(27)が成立する。すなわち、状態ベクトルx+ k−1のうち、姿勢(第2ベクトル)q+ k−1以外の要素を表すベクトル(第1ベクトル)β+ k−1と、推定された状態ベクトルx− kのうち、姿勢(推定第2ベクトル)q− k以外の要素を表すベクトル(推定第1ベクトル)β− kとは等しい。
式(27)が成立する場合、式(13)から明らかなように、シグマポイントχ+ k−1(i)のうちベクトルβ+ k−1に相当する要素と、シグマポイントの推定値χ− k(i)のうちベクトルβ− kに相当する要素とは、等しくなる。
従って、式(27)が成立する場合、状態ベクトルx+ k−1の推定誤差の共分散P+ k−1のうち、ベクトルβ+ k−1の推定誤差の共分散を表す成分と、推定された状態ベクトルx− kの推定誤差の共分散P− kのうち、ベクトルβ− kの推定誤差の共分散を表す成分とは、プロセスノイズuk−1の共分散Qを考慮しなければ、等しいものと看做すことができる(段落[0046]参照)。
When Expression (27) holds, as is clear from Expression (13), the element corresponding to the vector β + k−1 in the sigma point χ + k−1 (i) and the estimated value χ − of the sigma point vector of k (i) β - the corresponding element k, equal.
Therefore, when Expression (27) holds, the component representing the covariance of the estimation error of the vector β + k−1 out of the covariance P + k−1 of the estimation error of the state vector x + k−1 and the estimation state vector x - estimation error covariance P of k - among k, the vector beta - the component representing the covariance of the estimation error of k, to be considered the covariance Q of the process noise u k-1 Can be considered equal (see paragraph [0046]).
ここで、共分散Pkを、式(28)に示すように、行列Pqq,k、行列Pqβ,k、行列Pqβ,k T、及び行列Pββ,kの、4つの行列に分割する。行列Pqq,kは、姿勢qkに基づいて定められる、4行4列の行列である。行列Pqβ,kは、姿勢qkとベクトルβkとに基づいて定められる、4行11列の行列である。行列Pββ,kは、ベクトルβkに基づいて定められる、11行11列の行列である。具体的には、行列Pββ,kは、ベクトルβkの推定誤差の共分散を表す行列である。
また、プロセスノイズukの共分散Qを、式(29)のように、4行4列の行列Qqq、4行11列の行列Qqβ、及び11行11列の行列Qββを用いて表す。
式(6)に示したとおり、プロセスノイズukは、状態ベクトルxkを状態遷移モデルに適用する際に生じるプロセスノイズであり、式(29)に示す共分散Qは、当該プロセスノイズukの共分散を表す。従って、式(29)に現れる、行列Qββは、状態ベクトルxkを状態遷移モデルに適用する際に生じるプロセスノイズのうち、状態ベクトルxkの中で姿勢qk以外の要素を表すベクトルβkに生じるプロセスノイズの共分散を表す。
このとき、行列P− ββ,kは、式(30)に示すように、行列P+ ββ,k−1と、行列Qββとの和として定められる。
As shown in equation (6), the process noise u k is the process noise that occurs when applying the state vector x k to the state transition model, the covariance Q shown in equation (29), the process noise u k Represents the covariance of. Therefore, the matrix Q ββ appearing in the equation (29) is a vector β representing an element other than the posture q k in the state vector x k among the process noise generated when the state vector x k is applied to the state transition model. represents the covariance of the process noise occurring in k .
In this case, the matrix P - ββ, k, as shown in equation (30), the matrix P + ββ, and k-1, is defined as the sum of the matrix Q ββ.
共分散P− kの全ての成分を式(15)に基づいて求める場合、n行1列のベクトルと1行n列のベクトルとを乗算してn行n列の行列を求める演算を、a回繰り返すことが必要になる。
一方、式(30)を用いる場合、行列P− ββ,kは、1ステップ前に(時刻T=k−1において)既に算出されている行列P+ ββ,k−1と、行列Qββとを単純に加算することにより算出される。そして、共分散P− kは、行列P− ββ,k以外の成分、すなわち、4行4列の行列P− qq,k及び4行11列の行列P− qβ,kのみを、式(15)による演算により求めればよい。この結果、15行15列の共分散P− kの全ての成分を、式(15)を用いて算出する場合に比べて、共分散算出部125における計算量を大幅に低減することが可能となる。
When all the components of the covariance P − k are obtained based on the equation (15), an operation for obtaining an n × n matrix by multiplying an
On the other hand, when Expression (30) is used, the matrix P − ββ, k is defined as the matrix P + ββ, k−1 that has been calculated one step before (at time T = k−1), the matrix Q ββ , Are simply added. Then, the covariance P - k is a matrix P - ββ, components other than k, namely, four rows and four columns of the matrix P - qq, k and 4 rows 11 columns of the matrix P - Q [beta], k only, formula (15 ). Consequently, the covariance P of 15 rows 15 columns - all components of k, as compared with the case of calculating using the equation (15), can be greatly reduced calculation amount in the
なお、状態遷移モデルにおいて、式(25)及び式(27)に示すように、姿勢q以外は、前の時刻から変化しないという前提を置いた場合であっても、状態ベクトルxkは、カルマンフィルタの演算において観測残差ekに基づいて更新される。従って、姿勢qk以外の要素を表すベクトルβkも、カルマンフィルタの演算を繰り返す中で、観測残差ekに基づいて真値に近づくように更新される。 Note that, in the state transition model, as shown in Expression (25) and Expression (27), the state vector x k is the Kalman filter even when it is assumed that there is no change from the previous time except for the posture q. It is updated based in the operation of the observation residuals e k. Accordingly, the vector β k representing elements other than the posture q k is also updated so as to approach the true value based on the observation residual e k while repeating the Kalman filter calculation.
[3.4. 観測モデル]
次に、観測モデル部126で行われる演算において用いられる観測モデルについて説明する。
[3.4. Observation model]
Next, the observation model used in the calculation performed by the
3次元角速度センサ90から出力される角速度データgの推定値γgyroは、角速度ωと、3次元角速度センサ90のオフセット推定値gOとを用いて、式(31)で与えられる。
また、地上に固定された座標系において、地磁気を表すベクトルは、式(32)で与えられる。従って、3次元磁気センサ70から出力される磁気データmの推定値γmagは、3次元磁気センサ70のオフセット推定値mOと、式(5)で示した行列B(q)とを用いて、式(33)で与えられる。
また、地上に固定された座標系において、重力を表すベクトルを重力加速度で正規化したベクトルは、式(34)で与えられる。従って、3次元加速度センサ80から出力される加速度データaの推定値γaccは、式(5)で示した行列B(q)を用いて、式(35)で与えられる。
従って、式(3)を式(8)の右辺第1項に代入し、式(31)、式(33)、及び式(35)を用いて式(8)の右辺第2項を表すことにより、式(8)を、以下の式(36)に変形することができる。このとき、観測残差ekは、式(36)により算出される。
[4. 結論]
以上に示したように、本実施形態に係る状態推定装置は、シグマポイントカルマンフィルタの演算において用いられる状態ベクトルxkを、姿勢qkと、ベクトルβkとに分離して表現し、状態ベクトルx+ k−1を構成する要素の一部であるベクトルβ+ k−1と、推定された状態ベクトルx− kを構成する要素の一部であるベクトルβ− kとが等しいという前提を置いた。
そして、本実施形態に係る状態推定装置は、推定された状態ベクトルx− kの推定誤差の共分散P− kのうちベクトルβ− kの推定誤差の共分散を表す成分を、状態ベクトルx+ k−1の推定誤差の共分散P+ k−1のうちベクトルβ+ k−1の推定誤差の共分散を表す成分と、プロセスノイズuk−1の共分散Qのうちベクトルβ+ k−1に生じるプロセスノイズの共分散を表す成分とを単純に加算することで算出した。
これにより、共分散P− kの全ての成分を式(15)に基づいて求める場合と比較して、シグマポイントカルマンフィルタの計算負荷を大幅に低減することが可能となり、状態推定装置の処理速度の向上、及び携帯機器1の低消費電力化が可能となった。
[4. Conclusion]
As described above, the state estimation device according to the present embodiment represents the state vector x k used in the calculation of the sigma point Kalman filter by separating it into the posture q k and the vector β k, and the state vector x + k-1 which is a part of the elements constituting the vector β + k-1, the state vector x is estimated - vector beta is a part of the elements constituting the k - and k is put premise that equal .
The state estimation apparatus according to the present embodiment, the estimated state vector x - estimation error covariance P of k - vector of k beta - a component representing the covariance of the estimation error of k, the state vector x + The component representing the covariance of the estimation error of the vector β + k−1 in the covariance P + k−1 of the estimation error of k−1 and the vector β + k− of the covariance Q of the process noise u k−1. It was calculated by simply adding the component representing the covariance of the process noise occurring in 1 .
Thus, the covariance P - all components of k as compared with the case of calculating based on equation (15), it is possible to greatly reduce the computational load of the sigma points Kalman filter, the processing speed of the state estimation device Improvement and reduction in power consumption of the
<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
上述した実施形態では、状態ベクトルxを構成する要素して、携帯機器1の姿勢q、地磁気の強さr、地磁気の伏角θ、携帯機器1の角速度ω、3次元角速度センサ90のオフセット推定値gO、及び3次元磁気センサ70のオフセット推定値mOを採用し、これら6個の状態変数をカルマンフィルタの演算における推定の対象としたが、本発明はこれに限定されるものでは無い。例えば、状態ベクトルxを、これら6個の状態変数のうちの一部の状態変数により構成し、当該一部の状態変数をカルマンフィルタの演算における推定の対象とするものであってもよい。
(1)
In the above-described embodiment, the elements constituting the state vector x include the posture q of the
(2)変形例2
上述した実施形態では、観測値ベクトルyを、3次元磁気センサ70から出力される磁気データm、3次元加速度センサ80から出力される加速度データa、及び3次元角速度センサ90から出力される角速度データgを要素とするベクトルとしたが、本発明はこれに限定されるものでは無く、磁気データm、加速度データa、及び角速度データgのうちの一部のみを利用して観測値ベクトルyを生成してもよい。
(2) Modification 2
In the above-described embodiment, the observed value vector y is converted from the magnetic data m output from the three-dimensional
(3)変形例3
上述した実施形態では、平均算出部124は、式(14)に示すように、推定された状態ベクトルx− kを、a個のシグマポイントの推定値χ− k(i)の重み付き平均として算出したが、本発明はこれに限定されるものではない。
例えば、平均算出部124は、推定された状態ベクトルx− kを構成する要素の一部であるベクトルβ− kを、式(14)を用いず、単純に、状態ベクトルx+ k−1を構成する要素の一部であるベクトルβ+ k−1と等しい値に設定してもよい。この場合、平均算出部124は、推定された状態ベクトルx− kのうち、姿勢q− kについてのみ、式(14)の重み付き平均を算出する演算により算出する。これにより、15次元のベクトルである推定された状態ベクトルx− kのうち、11次元のベクトルβ− kについては、単純な値の代入により算出することが可能となるため、推定された状態ベクトルx− kの算出に係る処理負荷を、大幅に低減することができる。
(3) Modification 3
In the embodiment described above, the
For example, the
1…携帯機器、KF…カルマンフィルタ、121…遅延部、122…シグマポイント生成部、123…状態遷移モデル部、124…平均算出部、125…共分散算出部、126…観測モデル部、127…平均処理部、127…推定観測値ベクトル算出部、128…カルマンゲイン付与部、131…減算器、132…加算器、133…減算器、140…推定状態ベクトル算出部、150…推定観測値ベクトル算出部、K…カルマンゲイン、xk…状態ベクトル、x− k…推定された状態ベクトル、qk…姿勢、βk…ベクトル、Pk…状態ベクトルの推定誤差の共分散、Q…プロセスノイズの共分散、ek…観測残差、yk…観測値ベクトル、y− k…推定された観測値ベクトル。
DESCRIPTION OF
Claims (5)
前記推定状態ベクトルの推定誤差の共分散を算出する共分散算出部と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する推定観測値ベクトル算出部と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する観測残差生成部と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する更新部と、を備え、
前記推定状態ベクトル算出部は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、
前記共分散算出部は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタ。 Based on the state vector having the first vector and the second vector as elements and the covariance of the estimation error of the state vector, using the state transition model, the estimated first vector and the estimated second vector are elements. An estimated state vector calculation unit for calculating an estimated state vector;
A covariance calculation unit for calculating a covariance of an estimation error of the estimated state vector;
An estimated observation value vector calculation unit for calculating an estimated observation value vector from the state vector using the state transition model and the observation model;
An observation residual generation unit that calculates an observation residual based on the estimated observation vector and an observation vector that includes observation values output from a plurality of external sensors;
An update unit that calculates a vector obtained by updating the state vector based on the observation residual and the estimated state vector;
The estimated state vector calculation unit sets the estimated first vector to a value equal to the first vector,
The covariance calculation unit includes a component representing a covariance of the estimation error of the first vector among covariances of the estimation errors of the state vector, and a covariance of process noise generated in the state vector in the state transition model. Of these, the component representing the covariance of the process noise generated in the first vector is added to calculate the component representing the covariance of the estimated error of the estimated first vector among the estimated error covariance of the estimated state vector. A Kalman filter characterized by that.
前記状態ベクトルと、前記状態ベクトルの推定誤差の共分散とに基づいて、複数のシグマポイントを生成するシグマポイント生成部と、
前記状態遷移モデルを用いて、前記複数のシグマポイントから、複数の推定シグマポイントを算出する状態遷移モデル部と、
前記推定状態ベクトルを算出する平均算出部と、を備え、
前記状態遷移モデル部は、
前記推定シグマポイントのうち、前記推定第2ベクトルを算出するために用いられる要素以外の要素を、前記シグマポイントのうち、前記第1ベクトルに基づいて生成された要素と等しい値に設定し、
前記平均算出部は、
前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、前記推定第2ベクトルを、前記複数の推定シグマポイントに基づいて算出することを特徴とする、
請求項1に記載のカルマンフィルタ。 The estimated state vector calculation unit includes:
A sigma point generator that generates a plurality of sigma points based on the state vector and the covariance of the estimation error of the state vector;
A state transition model unit that calculates a plurality of estimated sigma points from the plurality of sigma points using the state transition model;
An average calculation unit for calculating the estimated state vector,
The state transition model part is:
Of the estimated sigma points, elements other than the elements used for calculating the estimated second vector are set to values equal to the elements generated based on the first vector of the sigma points,
The average calculator is
The estimated first vector is set to a value equal to the first vector, and the estimated second vector is calculated based on the plurality of estimated sigma points.
The Kalman filter according to claim 1.
第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する工程と、
前記推定状態ベクトルの推定誤差の共分散を算出する工程と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する工程と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する工程と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する工程と、を備え、
前記推定状態ベクトルを算出する工程は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、
前記推定状態ベクトルの推定誤差の共分散を算出する工程は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御方法。 A Kalman filter control method for estimating a system state,
Based on the state vector having the first vector and the second vector as elements and the covariance of the estimation error of the state vector, using the state transition model, the estimated first vector and the estimated second vector are elements. Calculating an estimated state vector;
Calculating an estimated error covariance of the estimated state vector;
Calculating an estimated observation value vector from the state vector using the state transition model and the observation model;
A step of calculating an observation residual based on the estimated observation vector and an observation vector having elements of observation values output from a plurality of external sensors;
Calculating a vector obtained by updating the state vector based on the observation residual and the estimated state vector, and
The step of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector;
The step of calculating a covariance of the estimation error of the estimated state vector includes a component representing a covariance of the estimation error of the first vector among the covariance of the estimation error of the state vector, and the state in the state transition model. Among the process noise covariances generated in the vector, a component representing the process noise covariance generated in the first vector is added to estimate the estimated first vector out of the covariances of the estimated state vector estimation errors. A method of controlling a Kalman filter, characterized in that a component representing an error covariance is calculated.
第1ベクトル及び第2ベクトルを要素とする状態ベクトルと、前記状態ベクトルの推定誤差の共分散と、に基づいて、状態遷移モデルを用いて、推定第1ベクトル及び推定第2ベクトルを要素とする推定状態ベクトルを算出する処理と、
前記推定状態ベクトルの推定誤差の共分散を算出する処理と、
前記状態遷移モデル及び観測モデルを用いて、前記状態ベクトルから、推定観測値ベクトルを算出する処理と、
前記推定観測値ベクトルと、外部の複数のセンサから出力される観測値を要素とする観測値ベクトルとに基づいて、観測残差を算出する処理と、
前記観測残差と、前記推定状態ベクトルとに基づいて、前記状態ベクトルを更新したベクトルを算出する処理と、をコンピュータに実行させ、
前記推定状態ベクトルを算出する処理は、前記推定第1ベクトルを、前記第1ベクトルと等しい値に設定し、
前記推定状態ベクトルの推定誤差の共分散を算出する処理は、前記状態ベクトルの推定誤差の共分散のうち、前記第1ベクトルの推定誤差の共分散を表す成分、及び前記状態遷移モデルにおいて前記状態ベクトルに生じるプロセスノイズの共分散のうち、前記第1ベクトルに生じるプロセスノイズの共分散を表す成分を加算して、前記推定状態ベクトルの推定誤差の共分散のうち、前記推定第1ベクトルの推定誤差の共分散を表す成分を算出する、ことを特徴とするカルマンフィルタの制御プログラム。
A Kalman filter control program that estimates the state of a system,
Based on the state vector having the first vector and the second vector as elements and the covariance of the estimation error of the state vector, using the state transition model, the estimated first vector and the estimated second vector are elements. A process of calculating an estimated state vector;
Processing for calculating a covariance of an estimation error of the estimated state vector;
A process of calculating an estimated observation value vector from the state vector using the state transition model and the observation model;
A process for calculating an observation residual based on the estimated observation vector and an observation vector having elements of observation values output from a plurality of external sensors;
Based on the observation residual and the estimated state vector, a computer that calculates a vector obtained by updating the state vector is executed,
The process of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector,
The processing for calculating the covariance of the estimation error of the estimated state vector includes the component representing the covariance of the estimation error of the first vector in the covariance of the estimation error of the state vector, and the state in the state transition model. Among the process noise covariances generated in the vector, a component representing the process noise covariance generated in the first vector is added to estimate the estimated first vector out of the covariances of the estimated state vector estimation errors. A control program for a Kalman filter, characterized in that a component representing an error covariance is calculated.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011201544A JP2013061309A (en) | 2011-09-15 | 2011-09-15 | Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011201544A JP2013061309A (en) | 2011-09-15 | 2011-09-15 | Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2013061309A true JP2013061309A (en) | 2013-04-04 |
Family
ID=48186086
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011201544A Withdrawn JP2013061309A (en) | 2011-09-15 | 2011-09-15 | Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2013061309A (en) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2017082662A (en) * | 2015-10-27 | 2017-05-18 | 富士通株式会社 | Engine torque estimation device, engine torque estimation system and engine torque estimation method |
CN114234970A (en) * | 2021-12-21 | 2022-03-25 | 华南农业大学 | Real-time measurement method and device for attitude of motion carrier |
CN114485723A (en) * | 2021-02-08 | 2022-05-13 | 北京理工大学 | High-rotation body air alignment method for adaptive robust matrix Kalman filtering |
CN117990112A (en) * | 2024-04-03 | 2024-05-07 | 中国人民解放军海军工程大学 | Unmanned aerial vehicle photoelectric platform target positioning method based on robust unscented Kalman filtering |
WO2024104446A1 (en) * | 2022-11-18 | 2024-05-23 | 优奈柯恩(北京)科技有限公司 | Device attitude estimation method and apparatus |
-
2011
- 2011-09-15 JP JP2011201544A patent/JP2013061309A/en not_active Withdrawn
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2017082662A (en) * | 2015-10-27 | 2017-05-18 | 富士通株式会社 | Engine torque estimation device, engine torque estimation system and engine torque estimation method |
CN114485723A (en) * | 2021-02-08 | 2022-05-13 | 北京理工大学 | High-rotation body air alignment method for adaptive robust matrix Kalman filtering |
CN114485723B (en) * | 2021-02-08 | 2024-02-27 | 北京理工大学 | High-rotation body air alignment method of self-adaptive robust matrix Kalman filtering |
CN114234970A (en) * | 2021-12-21 | 2022-03-25 | 华南农业大学 | Real-time measurement method and device for attitude of motion carrier |
WO2024104446A1 (en) * | 2022-11-18 | 2024-05-23 | 优奈柯恩(北京)科技有限公司 | Device attitude estimation method and apparatus |
CN117990112A (en) * | 2024-04-03 | 2024-05-07 | 中国人民解放军海军工程大学 | Unmanned aerial vehicle photoelectric platform target positioning method based on robust unscented Kalman filtering |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Valenti et al. | A linear Kalman filter for MARG orientation estimation using the algebraic quaternion algorithm | |
US10976341B2 (en) | Multi sensor position and orientation measurement system | |
JP5934296B2 (en) | Calibrating sensor readings on mobile devices | |
Michel et al. | A comparative analysis of attitude estimation for pedestrian navigation with smartphones | |
JP3848941B2 (en) | Geomagnetic sensor attitude error compensation apparatus and method | |
JP5061264B1 (en) | Small attitude sensor | |
KR101922700B1 (en) | Method and Apparatus for calculation of angular velocity using acceleration sensor and geomagnetic sensor | |
JP2013064695A (en) | State estimating device, offset updating method, and offset updating program | |
JP2014089113A (en) | Posture estimation device and program | |
US20140222369A1 (en) | Simplified method for estimating the orientation of an object, and attitude sensor implementing such a method | |
JP2017207456A (en) | Attitude estimation device, attitude estimation method, control program, and recording medium | |
JP7025215B2 (en) | Positioning system and positioning method | |
CN110440827B (en) | Parameter error calibration method and device and storage medium | |
US20230366680A1 (en) | Initialization method, device, medium and electronic equipment of integrated navigation system | |
JP2012173190A (en) | Positioning system and positioning method | |
KR20140025319A (en) | Apparatuses and methods for dynamic tracking and compensation of magnetic near field | |
Troni et al. | Adaptive Estimation of Measurement Bias in Three-Dimensional Field Sensors with Angular Rate Sensors: Theory and Comparative Experimental Evaluation. | |
JP2013029512A (en) | System and method for portable electronic device that detect attitude and angular velocity using magnetic sensor and accelerometer | |
JP2013096724A (en) | State estimation device | |
JP2017166895A (en) | Electronic apparatus, sensor calibration method, and sensor calibration program | |
US11709056B2 (en) | Method and device for magnetic field measurement by magnetometers | |
Vertzberger et al. | Attitude adaptive estimation with smartphone classification for pedestrian navigation | |
JP2013061309A (en) | Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter | |
Hoang et al. | Measurement optimization for orientation tracking based on no motion no integration technique | |
Kim et al. | Fast algebraic calibration of MEMS tri-axis magnetometer for initial alignment using least square method |
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: 20141202 |