JP2013088162A - State estimation apparatus - Google Patents

Abstract

PROBLEM TO BE SOLVED: To enable the arithmetic operation of a nonlinear Kalman filter at high speed and with a reduced processing load.SOLUTION: A state estimation apparatus 3 comprises: a plurality of sensors including a three-dimensional magnetic sensor 70; a plurality of Kalman filters KF each estimating the state of a system by updating a state vector xby using the state vector xwith a plurality of state parameters representing the state of the system as elements and an observed value vector ywith output values from the plurality of sensors as elements; an initial value generator 200 which generates a plurality of initial vectors INI being different from each other and supplies the initial vectors to the plurality of Kalman filters KF as initial values of the state vector x; and a Kalman filter controller 400 which specifies a Kalman filter having highest estimation accuracy by evaluating the estimation accuracy of each of the plurality of Kalman filters KF and stops operating the other Kalman filters KF than the specified Kalman filter KF.

Description

  The present invention relates to a state estimation device.

  When calculating the direction of geomagnetism, the posture of an object, etc., multiple sensors that measure different physical quantities such as acceleration sensors and angular velocity sensors in addition to the geomagnetic sensor, rather than calculating only based on the output result from the geomagnetic sensor If the output results are integrated and calculated, an accurate value can be obtained in a short time.

  A method using a nonlinear Kalman filter is known 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. For example, Patent Document 1 discloses a posture angle measurement device in which a triaxial angular velocity sensor, a triaxial acceleration sensor, and a nonlinear Kalman filter are mounted. In Non-Patent Document 1, output signals from a 3-axis angular velocity sensor, a 3-axis acceleration sensor, and a 3-axis geomagnetic sensor are integrated using an extended Kalman filter or a sigma point Kalman filter using unscented transformation, and the posture is integrated. An estimation method is disclosed.

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. And an observation model for estimating a value to be measured (observed value). The nonlinear Kalman filter uses a difference (observation residual) between the estimated observation value and the observation value actually measured by the plurality of sensors to estimate a plurality of physical quantities representing the state of the dynamic system ( By sequentially updating the state vector having the state variable) as an element, the value indicated by each element of the state vector is brought close to the actual physical quantity (true value).
In this way, the nonlinear Kalman filter is an operation that sequentially updates the state vector using the state vector and the observation residual calculated based on the state vector, so that the initial value element of the state vector is changed from the true value. When set to a deviated value, it may take a long time for each element of the state vector to converge to a value close to the true value. It can also converge to a value.

  In Patent Document 2, different initial values are given to a plurality of Kalman filters to operate in parallel, and an initial value deviating from a true value is set by performing state estimation using a Kalman filter having the smallest observation residual. A technique for avoiding a divergent state (a state vector element greatly deviating from a true value) is disclosed.

Japanese Patent Laid-Open No. 9-5104 JP-A-4-169814

Wolfgang Gunthner, “Enhancing Cognitive Assistance Systems with Inertial Measurement Units”, Springer, 2008

By the way, since noise is superimposed on the observation residual, the Kalman filter selected at a certain time does not necessarily have the smallest observation residual at the next time. Therefore, there is a problem that, as in the conventional technique, if a Kalman filter that minimizes the observation residual is simply selected, an inappropriate one may be selected.
Furthermore, when a plurality of Kalman filters are operated in parallel as in the conventional technique, the time for convergence of the state vector can be shortened, but the processing load increases. That is, there is a problem that the time until the state vector converges and the processing load are in a trade-off relationship.

  The present invention has been made in view of the above-described points, and it is an object of the present invention to converge the state vector to an appropriate value at high speed and reduce the processing load related to the calculation of the nonlinear Kalman filter.

  In order to solve the above-described problem, a state estimation device according to the present invention includes a plurality of sensors that observe a system and output observation values, and a state that includes a plurality of state variables that represent the state of the system as elements. A plurality of different Kalman filters that estimate the state of the system by updating the state vector by using a vector and an observation value vector having observation values output from the plurality of sensors as elements. An initial value generation unit that supplies each of the plurality of initial vectors to each of the plurality of Kalman filters as an initial value of the state vector, and evaluates the estimation accuracy of the plurality of Kalman filters, Among the plurality of Kalman filters, a Kalman filter having the highest estimation accuracy is identified, and the identified Kalman filter is Ku, characterized in that it comprises a Kalman filter controller stops the operation of the other Kalman filters.

  The Kalman filter uses the state vector and the observation value vector to repeat the calculation for updating the state vector, thereby converging each element of the state vector to a value close to a true value and estimating the state of the system. Therefore, when the elements of the initial value of the state vector (that is, the initial vector) greatly deviate from the true value, it may take a long time for each element of the state vector to converge to a value close to the true value. Furthermore, each element of the state vector may converge to an inaccurate value different from the true value. The Kalman filter supplied with such an initial vector has a low estimation accuracy and cannot accurately estimate the state of the system.

According to the present invention, the state estimation device supplies a plurality of different initial vectors to a plurality of Kalman filters, and operates the plurality of Kalman filters in parallel. When the state estimation device generates a plurality of different initial vectors, it is more likely that an initial vector having a value close to the true value as an element is included as compared with the case of generating one initial vector. A Kalman filter to which an initial vector whose elements are close to the true value is supplied can accurately and quickly converge each element of the state vector to the true value, so that the state accuracy of the system is high.
Then, the state estimation device identifies a Kalman filter having the highest estimation accuracy from among the plurality of Kalman filters, performs state estimation using the identified Kalman filter, and stops Kalman filters other than the identified Kalman filter. Therefore, according to the state estimation apparatus according to the present invention, accurate and high-speed state estimation is possible, and the processing load related to state estimation can be reduced.

  In the state estimation device described above, the Kalman filter calculates an observation residual based on the state vector and the observation value vector, calculates a covariance of the estimation error of the state vector, and performs the Kalman filter control. An observation residual comparison unit that selects a Kalman filter that minimizes an error evaluation value for evaluating an error between the state vector and the state of the system based on the observation residual output from each of the plurality of Kalman filters And calculating a convergence evaluation value representing the degree of convergence of the state vector based on the covariance of the estimation error of the state vector output by the Kalman filter selected by the observation residual comparison unit, and the convergence evaluation value is When the determination result of the convergence degree determination unit that determines whether or not the threshold value is equal to or less than the threshold value determination unit is positive, It is preferable to further include a stop control unit that identifies the Kalman filter selected by the observation residual comparison unit as the Kalman filter with the highest estimation accuracy and stops the operation of the other Kalman filters excluding the identified Kalman filter. .

According to this invention, the state estimation device specifies the Kalman filter with the highest estimation accuracy using the error evaluation value and the convergence evaluation value.
The error evaluation value is determined based on the observation residual and is a value representing an error between the value indicated by the state vector and the true value. That is, when the error evaluation value is small, each element of the state vector shows a value close to the true value. Therefore, the Kalman filter with the smallest error evaluation value can be regarded as a Kalman filter that most accurately estimates the state of the system.
On the other hand, the convergence evaluation value is determined based on the covariance of the state vector estimation error, and is a value representing the degree of convergence of the state vector. When the state vector has converged, if each element of the state vector shows a value close to the true value at a certain time, each element of the state vector may show a value close to the true value even after the certain time. Is expensive. That is, when the convergence evaluation value is small, the Kalman filter can stably estimate the system state.
By using such an error evaluation value and a convergence evaluation value, the system state can be estimated most accurately at a certain time, and the system state can be stably estimated after the certain time. A Kalman filter can be specified. That is, the state estimation device according to the present invention can accurately identify the Kalman filter with the highest estimation accuracy and perform accurate and stable state estimation.

  Further, in the state estimation device described above, the observation residual comparison unit calculates a maximum value in a predetermined period from a current time to a past time of a norm of at least some elements of the observation residual, Preferably, the calculation is performed for each of the plurality of Kalman filters, and the plurality of calculated Kalman filters that minimize the maximum value is selected as the Kalman filter that minimizes the error evaluation value.

The error evaluation value is a value calculated based on the observation residual. However, since the observation residual is a vector calculated based on an observation value vector having observation values output from a plurality of sensors as elements, noise is superimposed.
According to this invention, regarding the norm of at least some elements of the observation residual, the maximum value in the predetermined period is set as the error evaluation value. Therefore, even when the observation residual fluctuates due to observation noise or the like, the error evaluation value can be calculated while suppressing the influence of noise, and the magnitude of the error between the value indicated by the state vector and the true value can be calculated. It becomes possible to grasp accurately.

  In the state estimation apparatus described above, it is preferable that the convergence evaluation value is a matrix trace representing a covariance of the state vector estimation error.

In general, the eigenvalue of the covariance matrix is a value representing the magnitude of variance in the eigenvector direction corresponding to the eigenvalue. Therefore, the state vector estimation error covariance trace (that is, the sum of a plurality of eigenvalues of the covariance matrix) represents the state vector estimation error magnitude.
The Kalman filter updates the state vector in consideration of all state vector values from the initial state to the current time. Therefore, the state vector estimation error covariance is also updated in consideration of all covariance values from the initial state to the current time. Therefore, when the state vector estimation error covariance trace has a small value, it is considered that the state vector estimation error is stable at a small value (that is, the state vector is converged). I can do it.
Since the present invention employs a state vector estimation error covariance trace as the convergence evaluation value, it is possible to accurately determine whether or not the state vector is in a focused state.

  Further, in the state estimation device described above, before the Kalman filter control unit stops the other Kalman filter, the plurality of Kalman filters perform calculation using a first data type, and the Kalman filter control unit After stopping the other Kalman filters, the Kalman filter specified by the Kalman filter control unit performs an operation using a second data type having a data size larger than that of the first data type. Is preferred.

According to the present invention, when a plurality of Kalman filters operate in parallel, the plurality of Kalman filters perform an operation using the first data type having a small data size. Therefore, the state estimation apparatus according to the present invention can reduce the calculation load when operating a plurality of Kalman filters in parallel.
On the other hand, according to the present invention, when the specified Kalman filter operates alone, the specified Kalman filter performs an operation using the second data type having a large data size. Therefore, the state estimation apparatus according to the present invention can estimate the state of the system with high accuracy when the calculation is performed by one specified Kalman filter.

It is a block diagram which shows the structure of the portable apparatus which concerns on embodiment of this invention. It is a perspective view which shows the external appearance of a portable apparatus. It is a functional block diagram showing the function implement | achieved when the state estimation program which concerns on embodiment of this invention is performed. It is a flowchart for demonstrating operation | movement of the state estimation apparatus which concerns on embodiment of this invention. It is a key map for explaining an initial posture. It is a key map for explaining an initial posture. It is a key map for explaining an initial posture. It is a functional block diagram for demonstrating the function of the Kalman filter which concerns on embodiment of this invention.

<A. Embodiment>
Embodiments of the present invention will be described with reference to the drawings.

[1. Device configuration and software configuration]
FIG. 1 is a block diagram of a mobile device 1 according to an embodiment of the present invention, and FIG. 2 is a perspective view showing an appearance of the mobile device 1. The mobile device 1 has a function of rotating an image such as a map displayed on the screen included in the mobile device 1 in accordance with a change in the orientation of the mobile device 1 so that the orientation represented by the image follows the orientation of the real space. Prepare. This function is realized by performing a Kalman filter operation based on the outputs of various sensors included in the mobile device 1 and estimating the orientation of the mobile device 1 or the direction of geomagnetism viewed from the mobile device 1.

The mobile device 1 stores various programs and data such as a CPU 10 that is connected to various components via a bus and controls the entire apparatus, a RAM (storage unit) 20 that functions as a work area of the CPU 10, and a state estimation program 100. ROM 30, a communication unit 40 that executes communication, a display unit 50 that displays an image, and a GPS unit 60.
The portable device 1 detects a magnetic field such as geomagnetism and outputs magnetic data q, a three-dimensional acceleration sensor 80 that detects acceleration and outputs acceleration data a, and detects an angular velocity. A three-dimensional angular velocity sensor 90 that outputs angular velocity data g is provided.

The display unit 50 displays the orientation of the geomagnetism calculated based on the state of the system estimated by the CPU 10 executing the state estimation program 100, the orientation μ of the portable device 1, and the like by an image such as an arrow.
The GPS unit 60 receives a signal from a GPS satellite and generates position information (latitude, longitude) of the mobile device 1.

The three-dimensional magnetic sensor 70 is a sensor for detecting geomagnetism, and includes an X-axis magnetic sensor 71, a Y-axis magnetic sensor 72, and a Z-axis magnetic sensor 73. Each sensor can be configured using an MI element (magnetic impedance element), an MR element (magnetoresistance effect element), or the like. The magnetic sensor I / F 74 performs AD conversion on the output signal from each sensor and outputs magnetic data q. The magnetic data q is data represented as a vector having three components of the x axis, the y axis, and the z axis. More specifically, the magnetic data q represents the output value from the X-axis magnetic sensor 71 as an x-axis component in a coordinate system fixed to the portable device 1 that moves integrally with the three-dimensional magnetic sensor 70. The output value from the Y-axis magnetic sensor 72 is represented as a y-axis component, and the output value from the Z-axis magnetic sensor 73 is vector data represented as a z-axis component.
Since the three-dimensional magnetic sensor 70 is provided in the mobile device 1, in addition to geomagnetism, an internal magnetic field generated by components of the mobile device 1 and an external magnetic field generated by an external object outside the mobile device 1 are detected. . However, in this embodiment, it is assumed that the external magnetic field is small enough to be ignored.
Therefore, in order to calculate the direction and magnitude of the geomagnetism using the magnetic data q, it is necessary to perform a correction process for subtracting the component representing the internal magnetic field from the magnetic data q. In this correction process, a value to be subtracted (that is, a component representing the internal magnetic field) is called an offset of the three-dimensional magnetic sensor 70.

  The three-dimensional acceleration sensor 80 includes an X-axis acceleration sensor 81, a Y-axis acceleration sensor 82, and a Z-axis acceleration sensor 83. Each sensor may be any detection method such as a piezoresistive type, a capacitance type, or a heat detection type. The acceleration sensor I / F 84 AD-converts output signals from each sensor and outputs acceleration data a. This acceleration data a has the three components x-axis, y-axis, and z-axis of the resultant force of inertia and gravity in the coordinate system fixed to the portable device 1 that moves integrally with the three-dimensional acceleration sensor 80. Data represented as a vector. Therefore, if the mobile device 1 is in a stationary state or a constant velocity linear motion state, the acceleration data a is vector data indicating the magnitude and direction of the gravitational acceleration as viewed from the coordinate system fixed to the mobile device 1.

  The three-dimensional angular velocity sensor 90 includes an X-axis angular velocity sensor 91, a Y-axis angular velocity sensor 92, and a Z-axis angular velocity sensor 93. The angular velocity sensor I / F 94 AD-converts output signals from the sensors and outputs angular velocity data g. The angular velocity data g is vector data indicating the angular velocity around each axis.

  The CPU 10 estimates a plurality of physical quantities representing the state of the system such as the attitude of the mobile device 1 and the offset of the magnetic sensor by executing the state estimation program 100 stored in the ROM 30. That is, the portable device 1 functions as a state estimation device when the CPU 10 executes the state estimation program 100.

FIG. 3 is a functional block diagram showing functions realized by the CPU 10 executing the state estimation program 100 in the state estimation device.
As illustrated in FIG. 3, the state estimation device includes an initial value generation unit 200 that outputs a plurality of different initial vectors, a Kalman filter unit 300 that includes a plurality of Kalman filters and performs a Kalman filter operation, and a Kalman filter that controls the operation of the Kalman filter unit 300. An output information generation unit 500 that generates output information based on outputs from the control unit 400 and the Kalman filter unit 300 is provided.

The Kalman filter unit 300 includes ten Kalman filters KF, and each Kalman filter KF performs a nonlinear Kalman filter operation.
For convenience of description, when distinguishing the ten Kalman filters KF, the ten Kalman filters are denoted as KF [1], KF [2],..., KF [10], respectively. In addition, among the 10 Kalman filters, the m-th Kalman filter (m is a natural number satisfying 1 ≦ m ≦ 10) is denoted as KF [m].

Each of the ten Kalman filters KF [1] to KF [10] performs a nonlinear Kalman filter operation in either the first operation mode or the second operation mode.
Here, the first calculation mode is a mode in which the nonlinear Kalman filter is calculated using the first data type having the smallest data size among the data types representing real numbers that can be handled by the CPU 10 and the state estimation program 100. It is. The second calculation mode is a mode in which a nonlinear Kalman filter is calculated using a second data type having a data size larger than that of the first data type. That is, the second calculation mode is a calculation mode in which the calculation accuracy is high and the processing load is large as compared with the first calculation mode.

The calculation of the nonlinear Kalman filter is to estimate the state of the system by periodically updating the state vector x based on the observation residual z calculated using the state vector x and the observation value vector y described below. It is an operation to do. Although details will be described later, the observation residual z is a difference between the observed value vector y and the estimated observed value vector y ⁻ .
The calculation of the nonlinear Kalman filter also updates the covariance P of the estimation error of the state vector x at the same cycle as the update of the state vector.

The state vector x is a vector having a plurality of state variables as elements. A plurality of state variables, which are elements of the state vector x, respectively represent a plurality of physical quantities indicating a system state estimated by the nonlinear Kalman filter. That is, the state vector x is a vector representing the state of the system estimated by the nonlinear Kalman filter.
In this embodiment, as elements (state variables) of the state vector x, the attitude μ of the mobile device 1, the geomagnetic strength r, the geomagnetic dip angle φ, the angular velocity ω of the mobile device 1, and the offset estimated values of the angular velocity sensors 91 to 93. b _O and the offset estimated value c _O of the magnetic sensors 71 to 73 are adopted, and these are used as the targets of estimation in the calculation of the nonlinear Kalman filter.
The state vector x _{k at} time T = k 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.

Here, the geomagnetic strength r is a scalar, the geomagnetic dip angle φ is a scalar, the angular velocity ω is a three-dimensional vector, the angular velocity sensor offset estimate b _O is a three-dimensional vector, and the magnetic sensor offset The estimated value c _O is a three-dimensional vector.
In addition, the posture μ is expressed using quaternions. A quaternion is a four-dimensional number that represents the posture (rotation state) of an object. For example, a posture μ in which each axis of the coordinate system fixed to the portable device 1 and each axis of the coordinate system fixed on the ground coincide with each other is defined as a reference posture, and the reference posture is μ = (0, 0, 0). 1). At this time, the arbitrary posture μ of the mobile device 1 can be expressed as a posture rotated by an angle ψ with the direction of the unit vector ρ as the rotation axis with respect to the reference posture. The posture μ after rotation is given by Equation (2) using a quaternion.

On the other hand, the observed value vector y includes magnetic data q output from the three-dimensional magnetic sensor 70, acceleration data a output from the three-dimensional acceleration sensor 80, and angular velocity data g output from the three-dimensional angular velocity sensor 90. It is a vector as an element. Observation value vector y _{k at} time T = k is given by equation (3).

Each Kalman filter KF outputs a state vector x _k , an observation residual z _k , and a state vector estimation error covariance P _k that are periodically calculated as a result of the operation of the nonlinear Kalman filter. For convenience of explanation, the output from the m-th Kalman filter KF [m] is a state vector x _k [m], an observation residual z _k [m], and a state vector estimation error covariance P _k [m]. In this way, it is indicated with a symbol [m].
Although details will be described later, in the present embodiment, the state vector x _k output from each Kalman filter KF is strictly the state vector x ⁺ _k after update, and the state output from each Kalman filter KF. Strictly speaking, the vector estimation error covariance P _k is the updated state vector estimation error covariance P ⁺ _k .

The initial value generation unit 200 generates ten different initial vectors INI [1] to INI [10] based on various setting information stored in the ROM 30, and outputs these.
The ten initial vectors INI [1] to INI [10] are supplied to the ten Kalman filters KF [1] to KF [10], respectively, and the initial value of the state vector x (that is, the state at time T = 0). Applied as vector x ₀ ).

The m-th initial vector INI [m] includes an initial value μ ₀ [m] of the posture μ, an initial value r ₀ [m] of the geomagnetic strength r, an initial value φ ₀ [m] of the geomagnetic dip angle φ, Initial value ω ₀ [m] of angular velocity ω, initial value b _{O, 0} [m] of offset estimated value b _O of the angular velocity sensor, and initial value c _{O, 0} [m] of offset estimated value c _O of the magnetic sensor It is included as an element and is shown by the following equation (4).
A method for generating each element of the initial vector INI [m] will be described later.

  The Kalman filter control unit 400 has the highest estimation accuracy among the ten Kalman filters KF [1] to KF [10] based on the output values from the ten Kalman filters KF [1] to KF [10]. KF [Sel] is selected, and control is performed to stop the operations of the nine Kalman filters KF other than the selected Kalman filter KF [Sel].

The estimation accuracy of the Kalman filter KF is an index indicating how accurately and stably the system state is estimated by the Kalman filter KF. Specifically, the value indicated by the state vector x _k and the state vector value (error evaluation value) for evaluating the magnitude of the error between the true value, and an index represented by a value representing the degree of convergence of the state vector x _k (convergent evaluation value). The state where the estimation accuracy of the Kalman filter KF is high means that the error between the value indicated by the state vector x _k and the true value of the state vector is small (that is, the error evaluation value is small), and the state vector x _k converges. (That is, the convergence evaluation value is small). Here, the “true value” is a value representing an actual physical quantity corresponding to each element of the state vector x _k . The “true value of the state vector” is a vector whose elements are values (true values) representing actual physical quantities corresponding to the elements of the state vector x _k .
When the error between the value indicated by the state vector x _k and the true value of the state vector is small (that is, when each element of the state vector x _k indicates an accurate value close to the true value), the Kalman filter KF is time T = k Can be considered as accurately estimating the system.
Further, when the state vector x _k has converged, if each element of the state vector x _k indicates an accurate value close to a true value at time T = k, the state vector x can be changed even after time T = k. There is a high possibility that each element shows an accurate value close to the true value. That is, when the state vector _xk has converged, it can be considered that the Kalman filter KF stably estimates the state of the system.

Details of the Kalman filter control unit 400 will be described below.
Kalman filter control unit 400, the state vector x error between the true value of the value and state vector indicated by _k (error evaluation value) selects the smallest of the Kalman filter observation residual comparing unit 420, the state vector x _k has converged A degree-of-convergence determination unit 440 that determines whether or not it can be considered, and a stop control unit 460 that performs control to stop the operation of the Kalman filter KF.

The observation residual comparison unit 420 includes ten peak monitors PM [1] to PM [10] and a comparison unit 422.
As shown in the following equation (5), the mth peak monitor PM [m] uses the observation residual z _k [m] output from the mth Kalman filter KF [m] as time T = k−. Monitoring is performed for a predetermined period from τ ₀ to time T = k, and the maximum value of the norm of the observation residual z _k [m] in the period is output as the error evaluation value ^MAX z _k [m].

The comparison unit 422 has a minimum error evaluation value ^MAX z among the error evaluation values ^MAX z _k [1] to ^MAX z _k [10] output by the ten peak monitors PM [1] to PM [10]. _The selected value Sel indicating the Kalman filter KF [Sel] corresponding to _k [Sel] is output (where Sel is a natural number satisfying 1 ≦ Sel ≦ 10).
The observation residual comparison unit 420 executes the process only when the Kalman filter unit 300 outputs all of the observation residuals z _k [1] to z _k [10]. For example, CPU 10 is the Kalman filter unit 300, when outputting only one observation residuals _z k [m] of the observation residuals _{z k [1] ~z k [} 10] the observed residual comparing unit 420 Stop the operation.

As described above, the observation residual z _k is a value calculated using the state vector x and the observation value vector y. Based on the observation residual z _k , the value indicated by the state vector x and the true value of the state vector are calculated. The magnitude of the error from the value can be expressed. However, since noise is superimposed on the observed value vector y and is a value that varies with the passage of time T, the observation residual z _k also varies with the passage of time T.
Therefore, the observation residual comparison unit 420 monitors the observation residual z _k [m] for a predetermined period, and sets the maximum value of the norm of the observation residual z _k [m] in the predetermined period as the error evaluation value ^MAX z _k [m]. And using the error evaluation value ^MAX z _k [m], the magnitude of the error between the value indicated by the state vector x _k and the true value of the state vector is evaluated.
Thus, the observed residual comparing unit 420 can be reduced in evaluating the magnitude of the error between the value and the true value indicated by the state vector x _k, the effects of measurement error or noise of the various sensors, the Kalman filter KF which the smallest error between the true value of the value indicated by the state vector x _k and the state vector, it is possible to correctly select.

In the present embodiment, the maximum value of the norm of the observation residual z _k [m] in a certain period is adopted as the error evaluation value ^MAX z _k [m]. However, the present invention is limited to such a form. Instead, the maximum value in a predetermined period of the norm of some elements of the observation residual z _k [m] may be adopted as the error evaluation value. For example, the observation residual comparison unit 420 monitors, for a certain period, elements corresponding to the magnetic data q _k of the observation vector y _k among the elements constituting the observation residual z _k [m], and determines the norm of the element's norm. The maximum value may be adopted as the error evaluation value.

In the present embodiment, the error evaluation value ^MAX z determined based on the observation residual z _k [m] is used as a value for evaluating the magnitude of the error between the value indicated by the state vector x _k and the true value of the state vector. _{Although k} [m] is used, the present invention is not limited to this. Even if the error evaluation value is not ^MAX z _k [m], the error between the value indicated by the state vector x _k and the true value of the state vector is used. Any value can be used as long as the value can be evaluated.

The degree-of-convergence determination unit 440 selects among the covariances P ⁺ _k [1] to P ⁺ _k [10] of the estimation errors of the state vectors output from each of the ten Kalman filters KF [1] to KF [10]. The trace Tr (P ⁺ _k [Sel]) of the covariance P ⁺ _k [Sel] of the estimation error of the state vector output by the Kalman filter KF [Sel] indicated by the value Sel is adopted as the convergence evaluation value. Then, the convergence determination unit 440 determines whether or not the convergence evaluation value Tr (P ⁺ _k [Sel]) is equal to or less than the threshold value P _TH and outputs the determination result.

In general, the eigenvalue of the covariance matrix represents the magnitude of variance in the eigenvector direction corresponding to the eigenvalue. A trace (diagonal component) of a certain matrix represents the sum of a plurality of eigenvalues of the certain matrix. That is, the covariance matrix trace is a value representing the sum of the magnitudes of the variances in the direction of all eigenvectors of the covariance matrix. Therefore, the convergence evaluation value _{^{Tr (P + k [Sel]}} ), can be expressed magnitude of the estimation error of the state vector _{x k,} that is, the degree of convergence of the state vector _{x k.}
On the other hand, the state vector _{x k,} taking into account all of the state vector _x 0 _{~x k-1,} from the initial state T = 0 at time T = k-1, because it is a value to be updated, the estimation of the state vector The error covariance P ⁺ _k is also a value that takes into account the covariances P ⁺ _{0 to} P ⁺ _k−1 of the estimation errors of all state vectors from the initial state T = 0 to the time T = k−1. Therefore, the convergence evaluation value _{^{Tr (P + k [Sel]}} ) is in the period from the initial state T = 0 up to time T = k-1, using an estimation error of the state vector _x 0 _{~x k-1} Value. That is, if the convergence evaluation value _{^{Tr (P + k [Sel]}} ) becomes a small value can be regarded as an estimated error of the state vector x _k is stable at a small value. For example, if each element of the state vector x _k shows a value close to the true value at time T = k, each element of the state vector x can show a value close to the true value even after time T = k. High nature.
By using such a convergence evaluation value Tr (P ⁺ _k [Sel]), the degree of convergence of the state vector x _k of the Kalman filter KF [Sel] can be evaluated. When the convergence evaluation value Tr (P ⁺ _k [Sel]) is equal to or less than the threshold value P _TH (that is, when it can be considered that the state vector x _k has converged), the Kalman filter KF [Sel] Can be regarded as stably performing an accurate estimation of the state of the system.

In the present embodiment, as convergent evaluation value representing the degree of convergence of the state vector _{x k,} employing the trace of the covariance ^P ₊ k of the estimated error of the state vector _{^{[Sel] Tr (P + k}} [Sel]) However, the present invention is not limited to this, and any value other than the convergence evaluation value Tr (P ⁺ _k [Sel]) may be used as long as the value represents the degree of convergence of the state vector x _k. May be.
For example, the convergence determination unit 440 may employ the maximum eigenvalue of the state vector estimation error covariance P ⁺ _k [Sel] as the convergence evaluation value. That is, the convergence determination unit 440 may determine whether or not the maximum eigenvalue of the state vector estimation error covariance P ⁺ _k [Sel] is equal to or less than a predetermined threshold. In this case, if the maximum eigenvalue of the state vector estimation error covariance P ⁺ _k [Sel] is less than or equal to the threshold, the state vector x _k estimation error is stable at a small value (that is, the state vector x _k Is converged).

The convergence determination unit 440 executes processing only when the Kalman filter unit 300 outputs all of the covariances P ⁺ _k [1] to P ⁺ _k [10]. For example, when the Kalman filter unit 300 outputs only one covariance P ⁺ _k [m] among the covariances P ⁺ _k [1] to P ⁺ _k [10], the CPU 10 operates the convergence degree determination unit 440. Stop.

Stop control unit 460, the degree of convergence when the determination unit 440 a judgment result output is equal to or smaller than the threshold _{P TH,} the operation of nine Kalman filter KF except Kalman filter KF [Sel] showing the selected value Sel the comparison unit 422 outputs A control signal Ctr for stopping is output.

Based on the state vector x _k [Sel] output from the Kalman filter KF [Sel] selected by the Kalman filter control unit 400, the output information generation unit 500 determines the orientation of the mobile device 1 and the geomagnetism direction viewed from the mobile device 1. Generate output information such as
The output information generation unit 500 does not generate output information when the plurality of state vectors x _k [1] to x _k [10] are output from the Kalman filter unit 300, and does not generate output information. Only when a single state vector x _k [Sel] is output, output information is generated.

As described above, the calculation of the nonlinear Kalman filter calculates the state vector xk at time T = k based on the observation residual z _k and the state vector x _{k−1 at} time T = k−1. That is, the state vector _{x k,} taking into account all of the state vector leading from the initial state (time T = 0) at time _{T = k-1 x 0 ~x} k-1, is updated. Therefore, when the initial vector INI is set to a value greatly deviating from the true value of the state vector at time T = 0, the value indicated by the state vector x _k becomes the true value of the state vector even if the Kalman filter calculation process is performed. It takes a long time to converge to a close value, and the value indicated by the state vector x _k may not approach the true value of the state vector.
On the other hand, the state estimation apparatus according to the present embodiment supplies ten different initial vectors INI [1] to INI [10] to the ten Kalman filters KF [1] to KF [10], These ten Kalman filters KF [1] to KF [10] are operated in parallel. Then, the state estimation device identifies the Kalman filter KF [Sel] having the highest estimation accuracy, and outputs the output information based on the state vector x _k [Sel] output from the identified one Kalman filter KF [Sel]. Generate.
As described above, the state estimation apparatus according to the present embodiment is the initial vector set to a value close to the true value of the state vector at time T = 0 among the ten initial vectors INI [1] to INI [10]. to perform the state estimation by the Kalman filter KF [Sel] which operates based on the INI [Sel], it enabled the accurate and fast convergence of the state vector _{x k.} That is, according to the state estimation device according to the present embodiment, accurate and high-speed state estimation can be performed.

[2. Processing flow of state estimation device]
FIG. 4 is a flowchart for explaining the operation of the state estimation apparatus.
The processing shown in this flowchart is performed by the CPU 10 executing the state estimation program 100.

In step S101, the CPU 10 executes initial vector generation processing. Specifically, the CPU 10 generates a plurality of initial vectors INI [1] to INI [10].
That is, the CPU 10 functions as the initial value generation unit 200 by executing the initial vector generation process of step S101.

In step S102, the CPU 10 executes a Kalman filter parallel calculation process. Specifically, the CPU 10 performs the operation of the nonlinear Kalman filter using all of the plurality of Kalman filters KF [1] to KF [10], thereby obtaining the observation residuals z _k [1] to z _k [10]. The state vector estimation error covariances P ⁺ _k [1] to P ⁺ _k [10] are generated.
In the Kalman filter parallel calculation process, the CPU 10 performs the calculation of the nonlinear Kalman filter using the first data type (in the first calculation mode).

Since the Kalman filter parallel calculation process is a process of operating ten Kalman filters KF [1] to KF [10] in parallel, the processing load is large.
However, since the state estimation apparatus according to the present embodiment performs the Kalman filter parallel calculation processing in the first calculation mode using the first data type having the smallest data size, the parallel processing can be performed without greatly increasing the processing load. Can be executed.

In step S103, the CPU 10 executes an observation residual comparison process. Specifically, the CPU 10 determines the error evaluation values ^MAX z _k [1] to ^MAX z _k based on the observation residuals z _k [1] to z _k [10] generated in the Kalman filter parallel calculation processing in step S102. by calculating [10], the error between the true value of the values and the state vector indicating the state vector x _k to select the smallest Kalman filter KF [Sel].
That is, the CPU 10 functions as the observation residual comparison unit 420 by executing the observation residual comparison processing in step S103.

In step S104, the CPU 10 executes a convergence determination process. Specifically, the CPU 10 calculates the convergence evaluation value Tr (P ⁺ _k [Sel]) based on the covariance P ⁺ _k [Sel] of the estimation error of the state vector generated in the Kalman filter parallel calculation process in step S102. It is calculated, and it is determined whether or not the convergence evaluation value Tr (P ⁺ _k [Sel]) is equal to or less than the threshold value P _TH .
When the determination result is affirmative (that is, when the convergence evaluation value Tr (P ⁺ _k [Sel]) is equal to or less than the threshold value P _TH ), the CPU 10 advances the process to step S105. On the other hand, when the determination result is negative, the CPU 10 returns the process to step S102.
That is, the CPU 10 functions as the convergence determination unit 440 by executing the convergence determination process in step S104.

In step S105, the CPU 10 executes a Kalman filter stop process. Specifically, the CPU 10 stops the operations of the nine Kalman filters other than the Kalman filter KF [Sel].
That is, the CPU 10 functions as the stop control unit 460 by executing the Kalman filter stop process in step S105.

  Note that the CPU 10 also performs the Kalman filter parallel calculation process while executing the observation residual comparison process (step S103) and the convergence determination process (step S104). That is, in the Kalman filter stop process in step S105, the CPU 10 performs parallel calculation of the nonlinear Kalman filter using all the ten Kalman filters KF [1] to KF [10] until the CPU 10 stops the operation of the nine Kalman filters. Continue.

In step S106, the CPU 10 executes a Kalman filter single calculation process. Specifically, the CPU 10 performs a nonlinear Kalman filter calculation using only one Kalman filter KF [Sel]. In the Kalman filter single calculation process, the CPU 10 calculates the nonlinear Kalman filter by operating the Kalman filter KF [Sel] in the second calculation mode using the second data type.
Further, CPU 10, in the Kalman filter alone processing of step S106, the Kalman filter KF [Sel] is sequentially outputs the state vector _{x k} which periodically updated.
That is, the CPU 10 functions as the Kalman filter unit 300 by executing the Kalman filter parallel calculation process in step S102 and the Kalman filter single calculation process in step S106.

In step S107, the CPU 10 executes output information generation processing. Specifically, the CPU 10 generates output information based on the state vector x _k output in the Kalman filter single calculation process in step S106. The output information of the state estimation device according to the present embodiment is the attitude μ of the mobile device 1.
The CPU 10 functions as the output information generation unit 500 by executing the output information generation processing in step S107.
In the present embodiment, in the output information generation process in step S107, the CPU 10 generates the posture μ as output information. However, the present invention is not limited to this, and values other than the posture μ (for example, 3 The offset estimated value c _O ) of the dimensional magnetic sensor 70 may be generated as output information. In the present embodiment, the CPU 10 generates output information based only on the state vector x _k, but generates output information using information other than the state vector x _k (for example, the observed value vector y _k ). It doesn't matter. For example, CPU 10, based on the offset estimate c _O and magnetic data q of the magnetic sensor, may be calculated geomagnetic orientation in the coordinate system fixed on the ground.

Since the Kalman filter single calculation process in step S106 is a process that operates only one Kalman filter KF [Sel], the processing load is smaller than the Kalman filter parallel calculation process in step S102. Meanwhile, the output information generated in the output information generation processing in step S107, because it is generated based on the state vector x _k which is output in the Kalman filter alone processing, higher is preferably precision of calculation of the Kalman filter alone processing .
In the present embodiment, CPU 10 is the second operational mode using a second data type for performing Kalman filter alone operation processing, it is possible to calculate the high state vector x _k precision. As a result, the state estimation device can calculate accurate output information.

  Next, the CPU 10 determines whether or not an end condition for ending the state estimation program 100 is satisfied (step S108). The termination condition may be determined as appropriate based on the specifications of the mobile device 1 and the like. For example, the end condition may be that the mobile device 1 is turned off. When the end condition is satisfied, the CPU 10 ends the state estimation process shown in the flowchart. On the other hand, when the end condition is not satisfied, the CPU 10 advances the process to step S106.

[3. Initial vector generation]
As described above, the initial value generation unit 200 generates initial vectors INI [1] to INI [10]. The initial vectors INI [1] to INI [10] can be generated by any method as long as the initial vectors INI [1] to INI [10] are generated so as to include at least one value indicating the true value of the state vector at time T = 0. It doesn't matter.
Hereinafter, an example of a method for generating each element of the initial vectors INI [1] to INI [10] will be described.

First, the initial value μ ₀ [m] of the posture μ can be set as in the following Expression (6), for example.

Here, the meaning of the initial value μ ₀ [m] of the posture shown in Expression (6) will be described with reference to FIGS. 5 to 7 illustrate the initial posture using a teapot figure for easy understanding.
First, as shown in FIG. 5, an initial value μ ₀ [1] is determined. The initial value μ ₀ [1] may be set to any value, but in this embodiment, as shown in Expression (6), each of the basic postures (that is, each coordinate system fixed to the mobile device 1) The posture in which the axis and each axis of the coordinate system fixed on the ground coincide) is defined as an initial value μ ₀ [1].
Next, as shown in FIG. 5, the initial value μ ₀ [1] is rotated by 90 degrees, 180 degrees, and 270 degrees, respectively, with the z axis in the coordinate system fixed on the ground as the rotation axis, and three postures are generated. These three postures are assumed to have initial values μ ₀ [2], μ ₀ [3], and μ ₀ [4], respectively.
Further, as shown in FIG. 6, three postures are generated by rotating the initial value μ ₀ [1] by 90 degrees, 180 degrees, and 270 degrees, respectively, with the y axis in the coordinate system fixed on the ground as the rotation axis. These three postures are set as initial values μ ₀ [5], μ ₀ [6], and μ ₀ [7], respectively.
Similarly, as shown in FIG. 7, the initial value μ ₀ [1] is rotated by 90 °, 180 °, and 270 °, respectively, with the x axis in the coordinate system fixed on the ground as the rotation axis, and three postures are generated. These three postures are set to initial values μ ₀ [8], μ ₀ [9], and μ ₀ [10], respectively.
The initial value generation unit 200 generates more than ten initial value _mu 0 mutually different as shown in _{[1] ~μ 0 [10]} , respectively, as an element of the initial vector INI [1] ~INI [10] .

If the initial value μ ₀ of the posture μ is set to one predetermined fixed value (for example, μ ₀ = [ ₀ , ₀ , ₀ , 1]), the initial value μ ₀ is a true posture (true Value) is 180 degrees at the maximum.
In contrast, the initial value generation unit 200 according to the present embodiment determines ten initial postures by rotating the basic posture by 90 degrees. In this case, of the ten initial postures, the angle formed between the initial posture closest to the true posture and the true posture is 90 degrees or less. Therefore, the initial value generation unit 200 according to the present embodiment has an initial vector INI [m [m] whose initial value μ ₀ [m] is a value close to the true value regardless of the posture of the mobile device 1. ] Can be generated.

Next, an example of a method for generating the initial value c _{O, 0} of the offset estimated value c _O of the three-dimensional magnetic sensor 70 will be described.
As described above, the offset of the magnetic sensor is a vector representing the direction and magnitude of the internal magnetic field generated by the component mounted on the mobile device 1 in the coordinate system fixed to the mobile device 1. The internal magnetic field may be a larger magnetic field than the geomagnetism that is the detection target of the three-dimensional magnetic sensor 70. If the internal magnetic field is ignored, it is not possible to calculate an accurate geomagnetic direction. Therefore, in order to accurately calculate the direction of geomagnetism, it is necessary to accurately grasp the offset and subtract the offset from the magnetic data q output from the three-dimensional magnetic sensor 70.
However, when a predetermined fixed value (for example, the origin [0, 0, 0] ^T ) is applied to the initial value c _{O, 0} of the offset estimated value c _O of the three-dimensional magnetic sensor 70, There is a high possibility that the initial value c _{O, 0} of the offset estimated value c _O of the magnetic sensor is a value greatly deviating from the true offset (true value) of the magnetic sensor.

The initial value generation unit 200 according to the present embodiment calculates the initial value c _{O, 0} [m] of the offset estimated value c _O of the three-dimensional magnetic sensor 70 based on the initial value μ ₀ [m] of the posture μ. .
Specifically, the initial value generation unit 200 according to the present embodiment uses the initial value c _{O, 0} [m] of the estimated offset value c _O of the three-dimensional magnetic sensor 70 as the three-dimensional magnetic sensor 70 at time T = 0. Using the magnetic data q ₀ , the initial value μ ₀ [m] of the posture, the vector Bg shown in Equation (9), and the matrix B (μ) shown in Equation (8), ) Is set to the value given by. Here, the matrix B (μ) is a coordinate for expressing a vector expressed in the coordinate system fixed on the ground in the coordinate system fixed to the mobile device 1 when the mobile device 1 is in the posture μ. This is the matrix that performs the conversion. A vector Bg shown in Expression (9) is a vector representing the direction and magnitude of geomagnetism in the area where the mobile device 1 is used in the coordinate system fixed on the ground. In the present embodiment, the coordinate system fixed on the ground is determined so that the y-axis faces the magnetic pole north and the z-axis faces vertically upward.
The ROM 30 stores a lookup table LUT2 that stores the positional information and the geomagnetic vector Bg in association with each other. The initial value generation unit 200 refers to the lookup table LUT2 based on the position information generated by the GPS unit 60, acquires the geomagnetic vector Bg, and calculates the offset value of the magnetic sensor by calculating Expression (7). the initial value c _O of c _O, 0 to obtain a [m].

As described above, in the ten initial value _{μ 0 [1] ~μ 0 [} 10] , since include those a value close to the true position, 10 the initial value c _O, 0 [1 ] To c _{O, 0} [10] include values that are close to the true offset (true value) of the magnetic sensor. Therefore, the initial value generation unit 200 according to the present embodiment has the initial vector INI having the initial value c _{O, 0} [m] of the offset estimated value c _O of the three-dimensional magnetic sensor 70 having a value close to the true offset as an element. [M] can be generated.

Initial value _r 0 _[1] of the geomagnetic intensity r ~r 0 [10], and the initial value φ ₀ [1] of the geomagnetic dip φ ~φ ₀ [10], for example, is supplied from the GPS unit 60 It can be generated based on the position information. This is because if the position on the earth can be specified, the geomagnetic strength and the dip angle at that position can be known. Specifically, the ROM 30 stores a look-up table LUT1 that stores the positional information, the geomagnetic strength, and the dip angle in association with each other. The initial value generation unit 200 refers to the lookup table LUT1 and determines the geomagnetism strength and the dip angle corresponding to the position information, the initial values r ₀ [1] to r ₀ [10] of the geomagnetism strength r and the geomagnetism. Are set as initial values φ ₀ [1] to φ ₀ [10].
In addition, when the portable device 1 is in a place where radio waves from the satellite do not reach (for example, an underground shopping center), position information cannot be obtained from the GPS unit 60. In such a case, a typical value in a region where the mobile device 1 can be used may be adopted. The value is also stored in the lookup table LUT1. When the position information cannot be acquired, the initial value generation unit 200 reads a typical value from the lookup table LUT1. For example, for the mobile device 1 sold in Japan, the initial value r ₀ [1] to r ₀ [10] of the geomagnetism strength r and the geomagnetic dip angle based on the geomagnetism strength and the dip angle of Akashi City. calculates an initial value phi ₀ of _{φ [1] ~φ 0 [10} ].

Angular velocity initial value omega _{0 [1]} of ω ~ω _{0 [10],} for example, assuming that the portable device 1 is stationary is set to "0". In addition, the initial value b _{O, 0} [1] to b _{O, 0} [10] of the estimated offset value b _O of the angular velocity sensor is normally adjusted in the vicinity of “0”, and thus is set to “0”.

As described above, the state estimation device according to the present embodiment generates ten different initial vectors INI [1] to INI [10] so that at least one of the values that are close to the true value is included. Ten Kalman filters KF [1] to KF [10] are operated in parallel.
Therefore, the state estimation apparatus according to the present embodiment is supplied with the initial vector INI [m] indicating a value close to the true value of the state vector at time T = 0, and the state vector x _k is operated by the Kalman filter KF [m] that operates. Can be accurately and rapidly converged.

[4. Calculation using nonlinear Kalman filter]
Next, the calculation of the nonlinear Kalman filter will be described.

In general, the Kalman filter uses a state transition model that represents a change in the state of a dynamic system over time, from a state vector x _k−1 that indicates the state of the system at a certain time (time T = k−1), The state vector x _k after the unit time has elapsed (time T = k) is estimated. This estimated value is referred to as an estimated state vector x ⁻ _k . The Kalman filter estimates the observation value vector y _k from the estimated state vector x ⁻ _k 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. To do. This estimated value is referred to as an estimated observation value vector y ⁻ _k . Next, the Kalman filter estimated observed value vector y ^- and _k, calculates the difference between the observed value vector y _k to the actual observed value element as observed residuals z _k, based on the observation residuals z _k A Kalman gain _Kk is calculated. Thereafter, the Kalman filter updates the state vector x _k using the observation residual z _k , the Kalman gain K _k , and the estimated state vector x ⁻ _k , and calculates the updated state vector x ⁺ _k .
The Kalman filter repeats the update of the state vector x _k as described above, thereby converting each element of the state vector x _k to an accurate value close to a value (true value) that accurately represents the actual physical quantity. Get closer.

Although details will be described later, in this embodiment, a sigma point Kalman filter which is a nonlinear Kalman filter is used as the Kalman filter. The sigma point Kalman filter generates a plurality of sigma points χ _k-1 around the state vector x _k−1 , and applies these plurality of sigma points χ _k-1 to the state transition model. sigma points χ after ^- by calculating the average of _k, the estimated state vector x ^{- -} estimates the _k, estimated plurality of sigma points χ is a calculation for obtaining a _k. Therefore, strictly speaking, the estimated observation value vector y ⁻ _k is calculated based on a plurality of sigma points χ ⁻ _k existing around the estimated state vector x ⁻ _k .

In this embodiment, the state transition model of the nonlinear Kalman filter is given by the following equation (10), and the observation model is given by the following equation (11). In the present embodiment, the function f appearing in Expression (10) and the function h appearing in Expression (11) are nonlinear functions.

Here, the dimension of the state vector _{x k} represents a "dim (x)", the dimensions of the observed value vector _{y k} is represented as "dim (y)". In the present embodiment, dim (x) = 15 and dim (y) = 9. Further, the process noise w _k appearing in the expression (10) and the observation noise v _k appearing in the expression (11) are Gaussian noises centered on 0.
Equation (10) 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 is shown that it is estimated by adding the process noise w _k−1 .
Equation (11) 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.
Incidentally, representing the covariance of the process noise _{w k Q k,} the covariance of measurement noise _{v k} and _{R k.}

The observation residual z _{k at} time T = k is a vector determined based on the observed value vector y _k and the estimated observed value vector y ⁻ _k, and is represented by the following equation (12). As shown in the following equation (14), the Kalman filter uses the observed residual z _k , the estimated state vector x ⁻ _k , and the Kalman gain K _k shown in equation (13) to update the state vector x ^+. _k is calculated. Further, the Kalman filter updates the covariance P _k of the estimation error of the state vector x _k as shown in the following equation (15).
Here, 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 . P ^yy _k is a covariance of the observation residual z _k , and P ^xy _k is a mutual covariance matrix of the estimated state vector x ⁻ _k and the estimated observation value vector y ⁻ _k .

Note that the estimated observation value vector y ^- _k is a value calculated by applying the estimated state vector x ^- _k calculated using the state transition model to the observation model, as shown in Expression (11). . Therefore, the estimated observed value vector y ^- based on the observation residuals z _k which is a difference between the observed value vector y _k based on actual sensor output and _k, the estimated state vector x ^- was accurately represent the actual physical quantity and _k It is possible to evaluate the magnitude of the error from the value (the true value of the state vector).
As shown in Expression (14), by using the observation residual z _k , the estimated state vector x ⁻ _k is updated to the updated state vector x ⁺ _k , so that each element of the state vector x _k is true. You can get closer to the value.

  In the present embodiment, the ten Kalman filters KF [1] to KF [10] execute a sigma point Kalman filter operation using unscented transformation. Hereinafter, the calculation of the sigma point Kalman filter will be described in detail.

FIG. 8 is a functional block diagram showing functions realized by the state estimation device executing the Kalman filter KF [m]. Hereinafter, the operation of the Kalman filter KF [m] will be described with reference to FIG.
The ten Kalman filters KF [1] to KF [10] have the same configuration, and only the initial vectors INI [1] to INI [10] supplied from the initial value generation unit 200 are different.

The delay unit 301 delays the updated state vector x ⁺ _k output from the adder 311 by a unit time (a time corresponding to time T = k from time T = k−1), so that the state vector x ⁺ _k is delayed. ⁺ _K−1 is generated and output to the sigma point generation unit 302. In the first calculation (time T = 0), the delay unit 301 employs the initial value INI [m] output from the initial value generation unit 200 as the state vector x ⁺ _k−1 .
Further, the delay unit 301 delays the estimated error covariance P ⁺ _k of the updated state vector x ⁺ _k output from the subtractor 312 by unit time, thereby reducing the estimated error of the state vector x ⁺ _k−1 . A covariance P ⁺ _k−1 is generated and output to the sigma point generation unit 302.

The sigma point generation unit 302 uses “dim (x)” rows “dim (x)” columns of covariance matrices P ⁺ _k−1 and Q _k−1 to generate “2 dim (x) +1” sigma points. Is generated. Specifically, first, a vector σ _k shown in Expression (16) and Expression (17) is defined using a scaling parameter λ representing the spread of sigma points around the average for a plurality of sigma points. To do.

At this time, the sigma point generation unit 302 performs “2dim (x) +1” sigma expressed by the equations (18) and (19) based on the vector σ _k−1 and the state vector x ⁺ _k−1. Generate the point χ _k−1 .

The state transition model unit 303 applies each of “2dim (x) +1” sigma points χ _k−1 (j) at time T = k−1 to the state transition model, as shown in Expression (20). by estimate of "2dim (x) +1" number of sigma points at time T = k chi ^- calculating the _k (j).

Then, the average calculation unit 304, as shown in equation (21), the estimated value χ of "2dim (x) +1" number of sigma points at time T = k ^- to compute the average value of _k, the estimated A state vector x ^- _k is calculated.

The average calculation unit 304, shown in equation (22), the estimated state vector x ^- calculates the _k ^- covariance P of the estimation error of _k.

On the other hand, the observation model unit 305 applies each of “2dim (x) +1” sigma points χ _k (j) at time T = k to the observation model, as shown in Expression (23), “2 dim (x) +1” estimated observation values γ _k (j) are calculated.

Then, as shown in Expression (24), the average processing unit 306 calculates an average of “2dim (x) +1” estimated observation values γ _k (j) to obtain an estimated observation value vector y ⁻ _k . calculate.

Next, the average processing unit 306 calculates an observation residual covariance P ^yy _k shown in Expression (25).

The subtractor 307 calculates an observation residual z _k as a difference between the observed value vector y _k and the estimated observed value vector y ⁻ _k , as shown in Expression (12). The Kalman filter KF [m] outputs this observation residual z _k .

The Kalman gain assigning unit 308 calculates a mutual covariance matrix P ^xy _k shown in Expression (26). The Kalman gain applying unit 308, as shown in equation (13), calculates a covariance ^{P yy} _k observation residuals _{z k,} based on the cross-covariance matrix ^{P xy} _k, the Kalman gain _{K k} Then, the calculation of the second term (K _k z _k ) on the right side of Expression (14) is executed. In addition, the Kalman gain assigning unit 308 performs the calculation of the second term (K _k P ^yy _k K ^T _k ) on the right side of Expression (15).

The adder 311, as shown in equation (14), the estimated state vector x ^- addition and _k, equation output from the Kalman gain applying unit 308 the second term on the right side of (14) and _(K k _{z k)} Thus, the updated state vector x ⁺ _k is calculated. The Kalman filter KF [m] outputs the updated state vector x ⁺ _k .

Subtractor 312, as shown in equation (15), the estimated state vector x ^- estimation error covariance P of _k ^- _k and, the second term of the right side of equation (15) output from the Kalman gain applying unit 308 As a difference from (K _k P ^yy _k K ^T _k ), an updated state vector x ⁺ _k estimation error covariance P ⁺ _k is calculated (ie, the state vector estimation error covariance P _k is P ^- is updated to ^P _{+ k} from _k). The Kalman filter KF [m] outputs the covariance P ⁺ _k of the estimation error of the updated state vector x ⁺ _k .

  Next, the state transition model used in the calculation performed by the state transition model unit 303 will be described.

In the state transition model of the present embodiment, among the state variables that constitute the state vector x _k, the state transitions for a posture μ is defined by the equation (27). Expression (27) represents an operation for estimating the posture μ _k at time T = k after the elapse of the unit time from the posture μ _{k−1 at} time T = k−1. Here, μ ⁻ _k is an element corresponding to the state variable representing the posture μ in the estimated state vector x ⁻ _k .
Note that the operator Ω on the right side of Expression (27) is defined by Expression (28). Here, I _{3 × 3} represents a unit matrix of 3 rows and 3 columns. For the three-dimensional vector l = (l ₁ , l ₂ , l ₃ ), the operator [l ×] is defined by equation (29). Further, the measurement time interval (interval from time T = k−1 to time T = k) is represented by Δt, and the element corresponding to the state variable representing the angular velocity in the state vector x ⁺ _k after the update at time T = k. Is represented by ω ⁺ _k , the component Ψ ⁺ _k constituting the operator Ω is defined by the equation (30).

As shown in Expression (2), the posture μ is expressed by a quaternion, and the normalization condition || μ || = 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 an average of the estimated sigma point values χ ⁻ _k (i) as shown in the equation (21). Since it is calculated, the norm of the state vector x ⁺ _k−1 (before update) may be different from the norm of the state vector x ⁺ _k after update. Therefore, when performing the calculation of the sigma point Kalman filter, the norm of the posture μ ⁺ _k−1 which is an element of the state vector x ⁺ _k−1 (before update) and the posture which is an element of the state vector x ⁺ _k after update. There is a possibility of a different value from the norm of μ ⁺ _k . That is, when performing the sigma point Kalman filter calculation, there is a possibility that the normalization condition for the posture μ is not satisfied.
Therefore, when any calculation is performed on the posture μ, the result after the calculation is normalized by the size of the vector itself. In order to maintain the normalization condition more strictly, the posture μ _k among elements constituting the state vector x _k is limited to only the 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.

It is difficult to predict changes in the geomagnetic strength r and the geomagnetic dip angle φ. Therefore, in the state transition model according to the present embodiment, for the sake of convenience, the geomagnetic strength r _k and the dip angle φ _k at time T = k, and the geomagnetic strength r _k-1 and the dip angle φ _{k at} time T = k−1. Assume that ₋₁ is equal.
Similarly, the offset estimated value b _O of the angular velocity sensor, it is difficult to predict changes. Therefore, in the state transition model of the present embodiment, for convenience, time T = offset estimate b _O of the angular velocity sensor in _{k, K} and the time T = offset estimate value of the angular velocity sensor in _{k-1 b O, K-} 1 Are equal to each other.

Since the angular velocity ω of the mobile device 1 changes depending on how the user of the mobile device 1 moves the mobile device 1, the angular velocity ω _k−1 at time T = k ₋₁ is used at time T = k. It is difficult to formulate the angular velocity ω _k . Therefore, in the state transition model according to the present embodiment, for convenience, it is assumed that the angular velocity ω _k at time T = k is equal to the angular velocity ω _{k-1 at} time T = k−1.

As described above, the offset of the magnetic sensor is a vector expressing the direction and magnitude of the internal magnetic field generated by the components of the mobile device 1 in a coordinate system fixed to the mobile device 1. Therefore, the offset of the magnetic sensor does not change while the internal state of the mobile device 1 is constant. On the other hand, when the internal state of the mobile device 1 changes, for example, when the magnitude of the current flowing through the component mounted on the mobile device 1 changes, or the magnetization status of the component mounted on the mobile device 1 changes. In some cases, the offset of the magnetic sensor also changes. That is, the internal state of the mobile device 1 changes depending on the operation of the mobile device 1 by the user of the mobile device 1, the environment outside the mobile device 1, and the like. Therefore, it is difficult to predict the timing at which the offset of the magnetic sensor changes and the amount of change in the offset of the magnetic sensor, and using the estimated offset value c _{O, K-1} of the magnetic sensor at time T = k−1, It is difficult to formulate the estimated offset value c _{O, K} of the magnetic sensor at time T = k. Therefore, in the state transition model of the present embodiment, for convenience, time T = offset estimate c _O of the magnetic sensor in _{k, K} and the time T = offset estimate value of the magnetic sensor in _{k-1 c O, K-} 1 Are equal.

Thus, in the state transition model of the present embodiment, as shown in equation (31) shown below, among the plurality of state variables that constitute the state vector x _k, except the state variable representing the orientation mu, previous Presuppose that it does not change from the time.

In the Kalman filter calculation, each element of the state vector x _k is updated so as to approach the true value based on the observation residual z _k . Therefore, in the Kalman filter calculation, values other than the posture μ shown in Expression (31) do not necessarily change at all.

  Next, the observation model used in the calculation performed by the observation model unit 305 will be described.

The estimated value γ _gyro of the angular velocity data g output from the three-dimensional angular velocity sensor 90 is given by Expression (32) using the angular velocity ω and the estimated offset value b _O of the angular velocity sensor.

Further, a vector Bg representing geomagnetism in a coordinate system fixed on the ground is given by Expression (33). Therefore, the estimated value γ _mag of the magnetic data q output from the three-dimensional magnetic sensor 70 is obtained by using the offset estimated value c _O of the magnetic sensor and the matrix B (μ) shown in the expression (8). 34).

Further, in a coordinate system fixed on the ground, a three-dimensional vector _GRV obtained by normalizing a vector representing gravitational acceleration with the magnitude of gravitational acceleration is represented by the following equation (35). Therefore, the estimated value gamma _acc acceleration data a outputted from the three-dimensional acceleration sensor 80, using matrix B shown in equation (8) and (mu) and a vector _{G RV,} given by equation (36).

By substituting Equation (3) into the first term on the right side of Equation (12) and expressing the second term on the right side of Equation (12) using Equation (32), Equation (34), and Equation (36), Equation (12) can be transformed into the following equation (37). At this time, the observation residual z _k is calculated by Expression (37).

[5. Conclusion]
As described above, the state estimation apparatus according to the present embodiment supplies ten different initial vectors INI [1] to INI [10] to the ten Kalman filters KF [1] to KF [10]. Then, these ten Kalman filters KF [1] to KF [10] are operated in parallel. Then, the state estimation device identifies the Kalman filter KF [Sel] having the highest estimation accuracy, and performs state estimation using the identified Kalman filter KF [Sel]. Therefore, the state estimation apparatus according to the present embodiment can accurately and rapidly converge the state vector _xk , and can perform accurate and high-speed state estimation.

Moreover, the state estimation apparatus according to the present embodiment expresses the estimation accuracy of the Kalman filter KF using the error evaluation value ^MAX z _k [m] and the convergence evaluation value Tr (P ⁺ _k [Sel]).
The error evaluation value ^MAX z _k [m] evaluates the magnitude of the error between the value indicated by the state vector x _k and the true value of the state vector. Therefore, the state estimation apparatus according to the present embodiment has a state vector x _k whose elements are accurate values close to the true value, and can converge the state vector x _k to the true value in a short time. [Sel] can be selected.
In addition, the convergence evaluation value Tr (P ⁺ _k [Sel]) represents the degree of convergence of the state vector x _k . Therefore, the state estimation apparatus according to the present embodiment can identify the Kalman filter KF [Sel] that can stably perform accurate estimation of the system state.
As described above, the state estimation apparatus according to the present embodiment estimates the state of the system using one Kalman filter KF [Sel] specified based on the estimation accuracy of the Kalman filter KF. Estimates can be made.

In addition, the state estimation device according to the present embodiment performs Kalman filter parallel calculation processing in the first calculation mode and performs Kalman filter single calculation processing in the second calculation mode.
Thereby, the state estimation apparatus according to the present embodiment can perform accurate and highly accurate state estimation while reducing the processing load.

<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) Modification 1
In the above-described embodiment, the Kalman filter KF executes the calculation of the sigma point Kalman filter. However, the present invention is not limited to this, and the Kalman filter KF executes the calculation of the nonlinear Kalman filter other than the sigma point Kalman filter, such as an extended Kalman filter. You may do.
The state estimation apparatus according to the present invention specifies a Kalman filter KF having the highest estimation accuracy from among a plurality of Kalman filters KF, and estimates a system state using the specified Kalman filter KF. Then, regardless of what nonlinear Kalman filter operation is performed by the Kalman filter KF, the state vector of the Kalman filter KF with the highest estimation accuracy converges to an accurate value at high speed. That is, the state estimation apparatus according to the present invention can perform accurate and high-speed state estimation regardless of what nonlinear Kalman filter operation is performed by the Kalman filter KF.

(2) Modification 2
In the embodiment and the modification described above, the Kalman filter unit 300 includes ten Kalman filters KF [1] to KF [10]. However, the present invention is not limited to such a form, and the Kalman filter unit 300 includes: What is necessary is just to provide two or more Kalman filters KF. In this case, the initial value generation unit 200 only needs to generate a plurality of different initial vectors INI corresponding to the number of Kalman filters KF.
Note that the number of Kalman filters KF included in the Kalman filter unit 300 may be appropriately determined in consideration of the processing performance of the mobile device 1. For example, when the processing performance of the mobile device 1 is high, the Kalman filter unit 300 may include more than ten Kalman filters KF. In this case, the state vector x can be converged quickly and accurately. Further, when the processing performance of the portable device 1 is low, the Kalman filter unit 300 may include fewer than 10 Kalman filters. In this case, the processing load related to the Kalman filter parallel calculation processing can be reduced.

(3) Modification 3
In the embodiment and the modification described above, the state estimation device performs the Kalman filter parallel calculation processing in the first calculation mode and performs the Kalman filter single calculation processing in the second calculation mode. It is not limited to such an embodiment.
For example, the state estimation device may execute both the Kalman filter parallel calculation process and the Kalman filter single calculation process in the first calculation mode. In this case, the processing load related to the Kalman filter single calculation process can be reduced.
The state estimation device may execute both the Kalman filter parallel calculation process and the Kalman filter single calculation process in the second calculation mode. In this case, the state estimation device can perform highly accurate estimation.

(4) Modification 4
In the embodiment and the modification described above, the initial value generation unit 200 includes the initial value μ ₀ of the posture μ and the initial estimated offset value c _O of the three-dimensional magnetic sensor 70 among the plurality of state variables constituting the initial vector INI. A plurality of different initial vectors INI are generated by setting the value c _{O, 0} to be different from each other, but the present invention is not limited to such a method of generating the initial vector INI. For example, the initial value generation unit 200 may set all the state variables constituting the initial vector INI to different values.

(5) Modification 5
In the embodiment and the modification described above, the state estimation device includes the output information generation unit 500, but the present invention is not limited to such a configuration, and the state estimation device does not include the output information generation unit 500. May be configured. In this case, state estimation device may be designed to output a state vector x _k. In this case, various applications that cooperate with the state estimation program 100 may generate output information such as the posture and the direction of geomagnetism.

(6) Modification 6
In the embodiment and the modification described above, the mobile device 1 includes the three-dimensional magnetic sensor 70, the three-dimensional acceleration sensor 80, and the three-dimensional angular velocity sensor 90, but the present invention is not limited to such a form. The mobile device 1 may include any sensor. Note that the observed value vector y may be determined so that all or part of output values from a plurality of sensors included in the mobile device 1 are elements.
The present invention integrates the outputs of a plurality of sensors that measure different physical quantities, and performs accurate and high-speed state estimation in the calculation of a nonlinear Kalman filter that estimates the state of a dynamic system. Therefore, according to the present invention, accurate and high-speed state estimation can be performed regardless of what kind of sensor the state estimation apparatus includes.

(7) Modification 7
In the embodiment and the modification described above, as the state variables constituting the state vector x, the variables shown in Expression (1), that is, the attitude μ of the mobile device 1, the geomagnetic strength r, the geomagnetic dip angle φ, the mobile device 1, the offset estimated value b _O of the angular velocity sensors 91 to 93, and the offset estimated value c _O of the magnetic sensors 71 to 73 are used as estimation targets. It is not limited to Formula (1).
For example, a part of the variables shown in Expression (1) may be adopted as the state variable. Further, a variable other than the variable shown in Expression (1) may be adopted as the state variable.

DESCRIPTION OF SYMBOLS 1 ... Portable apparatus, 10 ... CPU, 70 ... Three-dimensional magnetic sensor, 80 ... Three-dimensional acceleration sensor, 90 ... Three-dimensional angular velocity sensor, 100 ... State estimation program, 200 ... Initial value production | generation part, 300 ... Kalman filter part, KF ... Kalman filter, 400 ... Kalman filter control unit, 420 ... observation residual comparison unit, 440 ... convergence degree determination unit, 460 ... stop control unit, 500 ... output information generation unit.

Claims (5)

A plurality of sensors for observing the system and outputting observation values,
Updating the state vector using a state vector whose elements are a plurality of state variables for representing the state of the system and an observation value vector whose elements are observation values output from the plurality of sensors, respectively. A plurality of Kalman filters for estimating the state of the system;
An initial value generating unit that generates a plurality of different initial vectors and supplies each of the plurality of initial vectors to each of the plurality of Kalman filters as an initial value of the state vector;
A Kalman filter control unit that evaluates the estimation accuracy of the plurality of Kalman filters, specifies a Kalman filter having the highest estimation accuracy among the plurality of Kalman filters, and stops operations of other Kalman filters excluding the identified Kalman filter;
A state estimation device comprising: The Kalman filter is
Calculating an observation residual based on the state vector and the observation value vector, and calculating a covariance of the estimation error of the state vector;
The Kalman filter control unit
An observation residual comparison unit that selects a Kalman filter that minimizes an error evaluation value for evaluating an error between the state vector and the state of the system, based on the observation residual output from each of the plurality of Kalman filters;
Based on the covariance of the estimation error of the state vector output by the Kalman filter selected by the observation residual comparison unit, a convergence evaluation value representing the degree of convergence of the state vector is calculated, and the convergence evaluation value is equal to or less than a threshold value A convergence determination unit that determines whether or not
When the determination result of the convergence determination unit is affirmative, the Kalman filter selected by the observation residual comparison unit is identified as the Kalman filter having the highest estimation accuracy, and the operations of other Kalman filters other than the identified Kalman filter are performed. A stop control unit for stopping;
The state estimation apparatus according to claim 1, comprising: The observation residual comparison unit
Calculating a maximum value of a norm of at least some elements of the observation residual in a predetermined period from a current time to a past time for each of the plurality of Kalman filters;
A plurality of calculated Kalman filters that minimize the maximum value are selected as Kalman filters that minimize the error evaluation value;
The state estimation apparatus according to claim 2, wherein: The convergence evaluation value is a matrix trace representing the covariance of the estimation error of the state vector.
The state estimation apparatus according to claim 2 or 3, wherein Before the Kalman filter control unit stops the other Kalman filter,
The plurality of Kalman filters perform an operation using a first data type;
After the Kalman filter control unit stops the other Kalman filter,
The Kalman filter specified by the Kalman filter control unit performs an operation using a second data type having a data size larger than that of the first data type. The state estimation apparatus according to item.

Images