JP2012132713A - State estimation device, state estimation method, and state estimation program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To converge a state vector at a high speed.SOLUTION: A state estimation device includes: non-linear Kalman filters KF1 to KF10; observation residual peak monitors PM1 to PM10 which detect the maximum value of norm of observation residual eof each filter; and an initial value generation section 130A which generates initial values INI1 to INI10 showing initial attitudes different from each other. When the initial values INI1 to INI10 are given, the non-linear Kalman filters KF1 to KF10 are operated in parallel. Then, a comparison section 110 compares the maximum values of the norms of the observation residual eand controls a selection section 120 so that output of the non-linear Kalman filter corresponding to the maximum value of the smallest norm is selected.

Description

  The present invention relates to a state estimation device, a state estimation method, and a state estimation program.

It is known to use a nonlinear Kalman filter such as an extended Kalman filter (EKF) as a method of integrating the outputs of sensors that measure different physical quantities in order to accurately know the posture of an object. 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 addition to the triaxial angular velocity sensor and triaxial acceleration sensor, there is no method for estimating the posture by integrating the output signals of the triaxial geomagnetic sensor using an extended Kalman filter or a sigma point Kalman filter (SPKF) using unscented transformation. It is disclosed in Patent Document 1.
In general, the Kalman filter is used to estimate the state of a certain dynamic system using observations including errors. For example, output values of a triaxial angular velocity sensor, a triaxial acceleration sensor, and a triaxial geomagnetic sensor are observation values including an error, and a state to be estimated is a posture.
Furthermore, Patent Document 2 gives different initial values to a plurality of Kalman filters, operates in parallel, detects a divergent state, and estimates a state using a non-divergent Kalman filter having the smallest observation residual. Techniques for performing are 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, noise is superimposed on the observation residual, and since the process until the state vector converges from the initial value can take various forms, the Kalman filter selected at a certain time is the most observed at the next time. The residual is not necessarily small. Therefore, there is a problem that, if the Kalman filter that minimizes the observation residual is always selected as in the conventional technique, an inappropriate one may be selected.
Furthermore, in the prior art, a plurality of different initial values are simply set, and no consideration has been given as to what initial values can be used to converge the state vector in a short time.
In addition, if the number of initial values and the number of Kalman filters that are operated in parallel are increased, the time to converge 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 solves the problem of appropriately selecting a Kalman filter, shortening the time to converge a state vector, and the like when operating a plurality of Kalman filters in parallel. To do.

  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 plurality of state variables that indicate the state of the system using a state transition model. Estimating a state vector, calculating an estimated observation value from the state vector estimated using an observation value model, calculating a difference between the estimated observation value and the observation values of the plurality of sensors as an observation residual, A plurality of Kalman filters for performing an operation of updating the state vector based on the observation residual and the estimated state vector; and a plurality of initial values different from each other as an initial state of the state vector, and the plurality of Kalman filters And an initial value generation unit supplied to each of the plurality of sensors, and an observation residual relating to at least one of the plurality of sensors included in the observation residual. An output unit that outputs the state vector of the specified Kalman filter, and the plurality of Kalman filters operate in parallel to calculate the observation residuals simultaneously, The output unit calculates, for each of the plurality of Kalman filters, a maximum value of an observation residual relating to the at least one sensor in a predetermined period from a current time to a past time by a predetermined time, and calculates the plurality of calculated maximum values. Of these, the Kalman filter corresponding to the smallest maximum value is specified as the Kalman filter in which the state vector converges most quickly.

  According to the present invention, a plurality of initial values of state vectors are set, and a plurality of different initial values are supplied to a plurality of Kalman filters operating in parallel. If the initial value deviates from the true value, it takes a long time for the state vector to converge. In the present invention, since different initial values are supplied to each Kalman filter and operated, some of them converge in a short time. Since the output unit specifies the Kalman filter that converges the state vector earliest based on the observation residual, the state vector can be converged at high speed.

  In addition, a small observation residual means that a state vector estimation error is small. However, paying attention to the instantaneous observation residual, there is a possibility of selecting an inappropriate one due to the influence of noise or the like. Since the present invention employs a Kalman filter so that the maximum value of the observation residual in a given period is reduced rather than the instantaneous value of the observation residual, a Kalman filter that can stably converge the state vector at high speed is selected. It becomes possible to do.

  In the state estimation device described above, the initial value generation unit includes a storage unit that stores the plurality of initial values, reads the plurality of initial values from the storage unit, and supplies the plurality of initial values to each of the plurality of Kalman filters. Also good. In order to converge the state vector at high speed, it is necessary to set an initial value close to the true value. However, there may be a case where the initial value cannot be predicted at all. In such a case, it is preferable to prepare a typical initial value in advance. According to the present invention, even when there is no clue for setting an initial value, the state vector can be converged at high speed.

  More specifically, the state vector of the system includes a state variable indicating the posture of the object, and the plurality of initial values are centered on each of three or more axes that intersect the reference posture of the object at an equal angle. It is preferable that the reference postures are a plurality of initial postures rotated by an equal angle. For example, the three or more axes are the X axis, the Y axis, and the Z axis. These axes are orthogonal to each other. Then, the reference posture may be rotated by 90 degrees, 180 degrees, and 270 degrees around each axis to determine three initial postures for each axis. When the reference posture and the plurality of initial postures are determined in this way, the postures determined by the initial values can be distributed evenly in the space. As a result, the state vector can be converged at high speed regardless of the posture of the object in the initial state.

  In addition, the state estimation device according to the present invention includes a plurality of sensors for observing the system and outputting observation values, and a state vector whose elements are a plurality of state variables indicating the state of the system using a state transition model. Estimating, calculating an estimated observation value from the state vector estimated using an observation value model, calculating a difference between the estimated observation value and the observation values of the plurality of sensors as an observation residual, A plurality of Kalman filters that execute an operation of updating the state vector based on the estimated state vector and a plurality of initial values different from each other as the initial state of the state vector are generated and supplied to each of the plurality of Kalman filters Based on an initial value generation unit and an observation residual relating to at least one of the plurality of sensors included in the observation residual, the state vector is generated. And an output unit that outputs a state vector of the identified Kalman filter. The plurality of Kalman filters operate in parallel to simultaneously calculate the observation residuals, and generate the initial value. The unit generates the plurality of initial values based on the observation values output from some or all of the plurality of sensors.

  According to the present invention, since a plurality of initial values are generated using observation values obtained by observing the system, the plurality of initial values can be brought close to a true value. That is, the range that can be taken as the initial value is limited by the observed value, and a plurality of initial values may be arranged within the limited range. For this reason, it becomes possible to converge the state vector at a higher speed. Alternatively, the initial value and the number of Kalman filters can be reduced, and the configuration can be simplified.

  Here, the output unit calculates, for each of the plurality of Kalman filters, a maximum value of an observation residual relating to the at least one sensor in a predetermined period from a current time to a past time by a predetermined time, It is preferable that the Kalman filter corresponding to the smallest maximum value among the maximum values is specified as the Kalman filter in which the state vector converges earliest. According to the present invention, it is possible to select a Kalman filter that can stably converge a state vector at high speed.

  In the state estimation apparatus described above, the state vector of the system includes a state variable indicating the posture of the object, and the sensor includes an acceleration sensor that detects acceleration of the object in three orthogonal axes, and the initial value generation unit Is based on the observation value of the acceleration sensor, and coincides with the inclination of the object with respect to the horizontal plane, and indicates each of a plurality of postures in which the object is rotated by an equal angle around an axis perpendicular to the horizontal plane. It is preferable to generate a plurality of initial values. Since the gravitational acceleration acts even if the object is fixed, it is possible to specify the direction in which the gravitational acceleration works with respect to the object from the output of the three-axis acceleration sensor. From the orientation, the inclination of the object with respect to the horizontal plane can be specified. However, it cannot be specified in which direction the object is facing. Therefore, a plurality of postures rotated by an equal angle around an axis perpendicular to the horizontal plane while maintaining an inclination with respect to the horizontal plane are set as initial postures. As a result, the state vector can be converged at high speed.

Further, as a specific aspect of the state estimation device described above, the plurality of sensors include a predetermined sensor that observes the system in three orthogonal axes, and the observation residual is an observation value of the predetermined sensor. The output unit includes a plurality of elements including a corresponding specific element, and the output unit calculates a maximum value of the specific element of the observation residual in the predetermined period for each of the plurality of Kalman filters, It is preferable that the Kalman filter corresponding to the smallest maximum value is specified as the Kalman filter in which the state vector converges earliest.
According to the present invention, since the Kalman filter can be selected by paying attention to a part of the observation residual, the processing can be simplified.

  The present invention can also be understood as a method invention. This method is a state estimation method for estimating a state of the system based on observation values output from a plurality of sensors observing the system, and includes a state having a plurality of state variables indicating the state of the system as elements. A plurality of different initial values are generated as initial states of the vector, the state vector is estimated using a state transition model, an estimated observation value is calculated from the state vector estimated using the observation value model, and the estimated observation A difference between a value and an observation value of the plurality of sensors is calculated as an observation residual, and a Kalman filter process is performed to update the state vector based on the observation residual and the estimated state vector. Each of the plurality of sensors included in the observation residual in a predetermined period from the current time to a past time by a predetermined time. A maximum value for an observation residual relating to at least one sensor is calculated for each of the plurality of Kalman filter processes, and a Kalman filter process corresponding to the smallest maximum value among the maximum values calculated for each of the plurality of Kalman filter processes, The state vector is specified as the Kalman filter process that converges the earliest, and the specified state vector of the Kalman filter process is output.

  Furthermore, the present invention can also be understood as a program invention. This program is a state estimation program used in a state estimation device that estimates the state of the system based on observation values output from a plurality of sensors that observe the system, and a plurality of states indicating the state of the system A process for generating a plurality of initial values different from each other as an initial state of a state vector having a variable as an element, estimating the state vector using a state transition model, and estimating observation from the state vector estimated using an observation value model Kalman filter processing for calculating a value, calculating a difference between the estimated observation value and the observation values of the plurality of sensors as an observation residual, and updating the state vector based on the observation residual and the estimated state vector Processing in parallel for each of the plurality of initial values and a predetermined period from the current time to a past time by a predetermined time And calculating a maximum value for an observation residual relating to at least one of the plurality of sensors included in the observation residual in each of the plurality of Kalman filter processes, and calculating each of the plurality of Kalman filter processes. Causes a computer to execute a process for identifying a Kalman filter process corresponding to the smallest maximum value among the maximum values as a Kalman filter process in which the state vector converges earliest and a process for outputting the identified state vector of the Kalman filter process .

  Further, the state estimation method according to the present invention is a method for estimating the state of the system based on observation values output from a plurality of sensors observing the system, from a part or all of the plurality of sensors. Based on the output observation value, a plurality of different initial values are generated as initial states of a state vector having a plurality of state variables indicating the state of the system, and the state vector is generated using a state transition model. Estimating, calculating an estimated observation value from the state vector estimated using an observation value model, calculating a difference between the estimated observation value and the observation values of the plurality of sensors as an observation residual, A Kalman filter process for updating the state vector based on the estimated state vector is executed in parallel for each of the plurality of initial values, and is included in the observation residual A maximum value for an observation residual relating to at least one of the plurality of sensors is calculated for each of the plurality of Kalman filter processes, and the smallest maximum value among the maximum values calculated for each of the plurality of Kalman filter processes The Kalman filter process corresponding to is specified as the Kalman filter process in which the state vector converges most quickly, and the specified state vector of the Kalman filter process is output.

  Further, a state estimation program according to the present invention is used for a state estimation device for estimating a state of the system based on observation values output from a plurality of sensors observing the system, and the plurality of sensors Processing for generating a plurality of initial values different from each other as an initial state of a state vector having a plurality of state variables indicating the state of the system based on the observation values output from a part or all of the state, and state transition The state vector is estimated using a model, an estimated observation value is calculated from the state vector estimated using an observation value model, and a difference between the estimated observation value and the observation values of the plurality of sensors is used as an observation residual. Kalman filter processing for calculating and updating the state vector based on the observed residual and the estimated state vector is performed on each of the plurality of initial values. And processing executed in parallel, processing for calculating a maximum value for an observation residual relating to at least one of the plurality of sensors included in the observation residual for each of the plurality of Kalman filter processing, A process for specifying a Kalman filter process corresponding to the smallest maximum value among the maximum values calculated for each Kalman filter process as a Kalman filter process in which the state vector converges earliest, and a process for outputting the specified state vector of the Kalman filter process Are executed by a computer.

  Note that the above-described program may be stored in advance in the memory of the apparatus, or may be updated by downloading via a communication network such as the Internet.

It is a block diagram which shows the structure of the mobile telephone which concerns on 1st Embodiment. It is a perspective view which shows the external appearance of a mobile telephone. It is a functional block diagram of the attitude | position estimation apparatus implement | achieved by running an attitude | position estimation program. It is explanatory drawing which shows the relationship between an initial attitude | position and a quaternion. It is explanatory drawing which shows the relationship between an initial attitude | position and a quaternion. It is explanatory drawing which shows the relationship between an initial attitude | position and a quaternion. It is a functional block diagram of a nonlinear Kalman filter. It is a functional block diagram of the attitude | position estimation apparatus which concerns on 2nd Embodiment. It is explanatory drawing for demonstrating a gravity acceleration vector.

Embodiments of the present invention will be described with reference to the drawings.
<1. First Embodiment>
FIG. 1 is a block diagram of the mobile phone according to the first embodiment of the present invention, and FIG. 2 is a perspective view showing the appearance of the mobile phone. The mobile phone 1 has a function of causing the azimuth represented by the image to follow the azimuth of the real space by rotating an image such as a map according to the direction of the screen that changes by changing the orientation of the mobile phone 1. Prepare. This function is realized by calculating the attitude of the mobile phone 1 by calculation using a Kalman filter based on the outputs of various sensors.

The mobile phone 1 includes a CPU 10 that is connected to various components via a bus and controls the entire apparatus, a RAM 20 that functions as a work area of the CPU 10, a ROM 30 that stores various programs and data, a communication unit 40 that performs communication, The display part 50 which displays an image, and the GPS part 60 are provided. The GPS unit 60 receives a signal from a GPS satellite and generates position information (latitude, longitude) of the mobile phone 1.
A geomagnetism detection unit 70 that detects geomagnetism and outputs geomagnetic data, an acceleration detection unit 80 that detects acceleration and outputs acceleration data, and an angular velocity detection unit 90 that detects angular velocity and outputs angular velocity data are provided.

  The geomagnetism detection unit 70 includes an X-axis geomagnetic sensor 71, a Y-axis geomagnetic sensor 72, and a Z-axis geomagnetic sensor 73. Each sensor can be configured using an MI element (magnetoimpedance element), an MR element (magnetoresistance effect element), or the like. The geomagnetic sensor I / F 74 performs AD conversion on the output signal from each sensor and outputs geomagnetic data. This geomagnetic data indicates a vector having a direction toward the north of the magnetic pole by three components of the x-axis, y-axis, and z-axis. In addition, since the geomagnetism detection unit 70 is provided in the mobile phone 1, the magnetism generated by components (such as a speaker) inside the mobile phone 1, the magnetism generated by an external object of the mobile phone 1, and the actual geomagnetism are combined. Detects a magnetic field. However, in the state transition model assumed in the posture estimation program described later, the magnetism generated by an external object is assumed to be negligibly small.

  The acceleration detection unit 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 of 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. This acceleration data indicates a vector having three components of an x-axis, a y-axis, and a z-axis, which is a resultant force of inertial force and gravity in an object-fixed system that moves integrally with the acceleration detector 80. Therefore, if the mobile phone 1 is in a stationary state or a constant velocity linear motion state, the acceleration data is vector data indicating the magnitude and direction of gravitational acceleration in an xyz coordinate space fixed with respect to the screen.

  The angular velocity detection unit 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 performs AD conversion on the output signal from each sensor and outputs angular velocity data. The angular velocity data indicates the angular velocity around each axis.

The CPU 10 specifies the posture of the mobile phone 1 by executing a posture estimation program (state estimation program) stored in the ROM 30. In this attitude estimation program, parallel nonlinear Kalman filter operations are executed. The nonlinear Kalman filter in the present embodiment, the quaternion q representing the orientation, geomagnetic strength r, geomagnetic dip theta, and the angular velocity omega, offset estimate g _o of the angular velocity _sensor, the offset estimate m _o of the geomagnetic sensor and the state variables, These are the targets of estimation. A state vector x _k having a state variable as an element at time k is given by the following equation (1).

The quaternion q is a mathematical expression representing the posture (rotation state) of the object, and is given as a four-dimensional number. Further, as elements of the state vector x _k, was moistened with offset estimate g _o of offset estimate m _o and the angular velocity sensor of the geomagnetic sensor superimposes the offset of the sensor 71 to 73 in the geomagnetic data, angular velocity data This is because the offsets of the sensors 91 to 93 are superimposed on each other. For example, since the cellular phone 1 has a built-in speaker equipped with a magnet, the geomagnetic sensor always detects the magnetic force of the magnet. Alternatively, the offset varies depending on the temperature change. In order to detect the exact position, by using the angular velocity data corrected by the corrected geomagnetic data and offset estimate g _o by offset estimate m _o, it is necessary to calculate the state vector x _k. The correction by the offset estimate _{m o} and offset estimate _{g o} is CPU10 executes.

Next, the observation target in the Kalman filter is the observation value m which is the output of the triaxial geomagnetic sensors 71 to 73, the observation value a which is the output of the triaxial acceleration sensors 81 to 83, and the triaxial angular velocity. This is an observation value g that is an output of the sensors 91 to 93. The observation values m, a, and g are vectors each composed of three elements, the X axis, the Y axis, and the Z axis. Output vector y _k of the Kalman filter at time k is given by Equation (2).

The mobile phone 1 functions as a posture estimation device when the CPU 10 executes a posture estimation program. FIG. 3 shows a functional block diagram of the posture estimation apparatus obtained by executing the program.
The posture estimation apparatus includes ten nonlinear Kalman filters KF1 to KF10 and observation residual peak monitors PM1 to PM10, a comparison unit 110, a selection unit 120, and an initial value generation unit 130A. Among these, the observation residual peak monitors PM1 to PM10, the comparison unit 110, and the selection unit 120 identify the nonlinear Kalman filter that the state vector converges earliest based on the observation residuals of the nonlinear Kalman filters KF1 to KF10, It functions as an output unit that outputs the state vector of the specified nonlinear Kalman filter.

Each of the nonlinear Kalman filters KF1 to KF10 estimates a state variable after a unit time has elapsed from a state variable indicating the state of the system at the current time, using the state transition model of the system related to the attitude of the mobile phone 1, and the state An observed value is estimated based on the estimated value of the variable, and a difference between the estimated observed value and the observed values of various sensors is calculated as an observation residual _{ek + 1} . The configurations of the nonlinear Kalman filters KF1 to KF10 are the same, and only given initial values INI1 to INI10 are different.

Observation residual peaks monitor PM1~PM10 monitors the observation residuals e _k, and outputs the maximum value of the norm of the observation residuals e _k in only a predetermined past period from the current time for a predetermined time.
Comparing unit 110 compares the output signal of the observation residual peak monitor PM1～PM10, identify the smallest of the maximum value of the norm of the observation residuals e _k of a predetermined time period, the norm of the observation residuals e _k is A control signal CTL that specifies the minimum nonlinear Kalman filter KF is generated.

More specifically, the three components corresponding to the geomagnetic sensor of the observation residuals e _k of the i-th filter at time k z _i, and _k. The smallest non-linear Kalman filter observation residuals e _k is considered to be estimating the appropriate orientation, but may be inappropriate when focusing only on the observation residuals e _k of a certain moment from being adopted. Therefore, the norm maximum value of the observation residuals e _k between the time (k-tau ₀₎ to time k focused on geomagnetism is calculated according to equation (3). This calculation is executed by the observation residual peak monitors PM1 to PM10. The comparison unit 110 generates the control signal CTL so as to select the i-th filter that minimizes this index.

The selection unit 120 selects and outputs one of the state vectors output from the nonlinear Kalman filters KF1 to KF10 according to the control signal CTL. This state vector includes the quaternion q representing the posture, the geomagnetic strength r, the geomagnetic depression angle θ, the angular velocity ω, the angular velocity sensor offset estimated value g _o , and the geomagnetic sensor offset estimation, as shown in the above-described equation (1). _{Let the} value _mo be a state variable.

  Appropriate selection of the initial value is important in an attitude estimation apparatus using a nonlinear Kalman filter. The initial value generation unit 130 </ b> A generates an initial value INI of the geomagnetism strength r and the dip angle θ based on the position information supplied from the GPS unit 60. This is because if the position on the earth can be specified, the strength r and the dip angle θ of the geomagnetism at that position can be known. Specifically, the ROM 30 stores a lookup table LUT1 in which the position information, the geomagnetic strength r and the dip angle θ are stored in association with each other, and the geomagnetic strength r corresponding to the position information is referenced with reference to this table. And the depression angle θ are read out. In addition, when the mobile phone 1 is in a place where radio waves from the satellite do not reach (for example, an underground mall), position information cannot be obtained from the GPS unit 60. In such a case, a typical value in a region where the mobile phone 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 130A reads a typical value from the lookup table LUT1. For example, for the mobile phone 1 sold in Japan, the geomagnetic strength r and the dip angle θ of Akashi City may be used as the initial INI.

  The initial value of the angular velocity ω may be set to 0 assuming rest. Offset estimate value g of the angular velocity sensor. Is usually adjusted to near zero, so it should be 0. Alternatively, assuming that the first measurement value is obtained in a stationary state, the value may be given as the initial value INI.

What is important is the estimated offset value m ₀ and the posture q of the geomagnetic sensor. The geomagnetic sensor is used to know the direction and magnitude of the geomagnetic vector. However, peripheral components such as the speaker of the mobile phone 1 generate a large magnetic field. In this case, the offset is usually much larger than the magnitude of geomagnetism. Therefore, it is desirable to avoid the same value in any case and to give a value as close to the true value as possible.
Assuming a certain posture, the posture q is shifted by 180 degrees in the worst case. When the difference between the initial value and the actual initial posture is large as described above, even when the state vector converges correctly, the speed is extremely slow, and the state variable may not converge to the correct value.
Therefore, in this embodiment, different initial values INI1 to INI10 are given to the ten nonlinear Kalman filters KF1 to KF10 to operate them in parallel.

Geomagnetic vector and gravity vector in the coordinate system fixed to the ground is assumed to be m _e, g _e respectively. Assuming that the object is stationary, the output each m _e of the magnetic sensor and the acceleration sensor in the sensor fixing of the coordinate _system, a reference posture of the posture as a g _e, q = (0 quaternion representing the reference attitude , 0, 0,1). An arbitrary posture can be expressed as a posture rotated by an angle ψ with the direction of the unit vector α as a rotation axis with respect to the reference posture. The quaternion q representing the posture after rotation is given by the following equation (4).

The following 10 types are prepared as quaternions q ₀ representing the initial posture. In FIG. 4 to FIG. 6, the initial posture is illustrated with a teapot graphic for easy understanding. The same applies to the mobile phone 1.
4) 1) q ₀ = (0, 0, 0, 1) shown in FIG. 4 is a reference posture (a part of the initial posture), and the spout of the teapot extends in the X-axis direction. In addition, 2) to 4) are obtained by rotating the teapot on the Z axis. 2) q ₀ = (0, 0, 1/2 ^1/2 , 1/2 ^1/2 ) is an attitude obtained by rotating the reference attitude 90 degrees on the Z axis. 2) can be obtained by substituting α = (0, 0, 1) and ψ = 90 into equation (4). The same applies to the other postures 3) to 10). 3) q ₀ = (0, 0, 1, 0) is an attitude obtained by rotating the reference attitude 180 degrees on the Z axis. 4) q ₀ = (0, 0, −1/2 ^1/2 , 1/2 ^1/2 ) is an attitude obtained by rotating the reference attitude on the Z axis by −90 degrees.
Further, FIG. 5 shows _{5) q 0 = (0,1 /} 2 1/2, 0, 1/2 1/2) is a posture obtained by rotating the reference attitude 90 degrees on the Y axis. 6) q ₀ = ( _{0, 1, 0, 0} ) is a posture obtained by rotating the reference figure 180 degrees on the Y axis. 7) q ₀ = (0, − 1/2 ^1/2 , 0, 1/2 ^1/2 ) is an attitude obtained by rotating the reference attitude on the Y axis by −90 degrees.
Further, 8) q ₀ = (1/2 ^1/2 , 0,0, 1/2 ^1/2 ) shown in FIG. 6 is a posture obtained by rotating the reference posture by 90 degrees on the X axis. 9) q ₀ = (1, _{0, 0, 0} ) is a posture obtained by rotating the reference posture 180 degrees on the X axis. 10) q ₀ = (− 1/2 ^1/2 , 0,0, 1/2 ^1/2 ) is an attitude obtained by rotating the reference attitude on the X axis by −90 degrees.

When the posture is q, the output of the geomagnetic sensor is B (q) m _e + m _b . M _e as described above, a geomagnetic vector in the coordinate system fixed to the ground. On the other hand, the geomagnetic sensor provided in the mobile phone 1 changes the strength of the geomagnetism detected in each of the XYZ axes when the posture of the mobile phone 1 changes. B (q) is a matrix used for converting the coordinate system fixed to the ground into the coordinate system of the geomagnetic sensor of the mobile phone 1 using the posture q, and is given by the following equation (5).

Even if the posture is changed, the relative position between the speaker and the geomagnetic sensor does not change, so the magnetic field generated by the speaker magnet is detected by the geomagnetic sensor regardless of the posture. This corresponds to a true offset m _b of the geomagnetic sensor to determine estimates.
The initial value m _o of the estimated value of the offset m _b of the geomagnetic _{sensor, k} is the observed value m ₀ of the geomagnetic sensor at time k = 0, using a geomagnetic vector m _typical areas that use the mobile phone 1, below It is given by the following expression (6).

In Equation (6), the term B (q ₀ ) m _typical is a geomagnetic component obtained by converting the geomagnetic vector m _typical into the coordinate axis of the geomagnetic sensor. The observed value m ₀ of the geomagnetic sensor is obtained by adding a geomagnetic component and an offset. Therefore, calculates the initial value _{m o, 0} of the estimated value of the offset _{m b} from the observed values _{m 0} by subtracting the geomagnetic component _{B (q 0) m typical.} The ROM 30 stores a lookup table LUT2 that stores positional information and the geomagnetic vector m _typical in association with each other. The initial value generation unit 130A, by which obtains geomagnetic vector m _typical with reference to a lookup table LUT2 based on the position information generated by the GPS unit 60 calculates Equation (6), the estimation of the offset m _b The initial value m _{o, 0} of the value is obtained.

As described above, the initial value generation unit 130A generates ten initial postures, generates initial values INI1 to INI10 corresponding to the respective initial postures, and supplies them to the nonlinear Kalman filters KF1 to KF10. In the nonlinear Kalman filter KF to which the initial value INI generated based on the initial posture close to the actual posture is supplied, the state vector converges in a short time. Whether or not the state vector has converged can be known from the observation residual _{ek + 1} .
In the present embodiment, the comparison unit 110 compares the maximum values of the observation residuals e _{k + 1} to select the most converged nonlinear Kalman filter KF among the nonlinear Kalman filters KF1 to KF10. As a result, an accurate posture can be calculated in a short time.

Next, the contents of the nonlinear Kalman filters KF1 to KF10 will be described. FIG. 7 is a functional block diagram of the nonlinear Kalman filter KF1. The other nonlinear Kalman filters KF2 to KF10 have the same configuration.
The nonlinear Kalman filter KF1 is configured by a sigma point Kalman filter, and a model of the system is given by the following equations (7) and (8).

Here, x _k is an n-dimensional state vector, and y _k is an m-dimensional observation value vector. As shown in Expression (1), the state vector x _k includes the posture q _k and the offset estimation value m _{o, k} of the geomagnetic sensor as state variables. The process noise u and the observation noise υ are Gaussian noises centered on zero. The equation (7), the state vector x _{k + 1} at time k + 1 substitutes the state vector x _k at time k to the function f, it is obtained by adding the process noise u _k at the result and the time k Show. Equation (8) indicates that the observed value vector y _k at time k is obtained by substituting the state vector x _k at time _k into the function h and adding the result to the observed noise υ _{k at} time k. Show. In the following description, it is assumed that the covariance of the process noise u _k is Q _k and the covariance of the observation noise is R _k .

Since the observation residual e _k is a difference between the observation value vector y _{k at} time k and a vector estimated using the observation model, it can be expressed by the following equation (9). The updating of the non-linear Kalman filter using the observation residuals e _k, given by the following expressions (10) and (11).

The subtraction of equation (9) is executed by the subtracter 260 shown in FIG. Further, the calculation of the second term on the right side of Expression (10) is executed by the Kalman gain assigning unit 270, and the addition of Expression (10) is executed by the adder 280. The delay unit 290 delays the output of the adder 280 until the next time.
The Kalman gain is given by the following equation (12).

In the sigma point Kalman filter, the sigma point generation unit 210 first generates (2n + 1) sigma points using an n-by-n covariance matrix. Expressions (13) and (14) are defined by using a scaling parameter λ that represents the spread of the sigma point around the mean.

Further, the sigma point is determined by Expression (15) and Expression (16). The calculations of Expressions (13) to (16) are executed by the sigma point generation unit 210. The sigma point generation unit 210 is supplied with the initial value INI1. In the first calculation of the sigma point Kalman filter, the sigma point generation unit 210 generates the sigma point using the initial value INI1 as the right side of the equation (15).

Furthermore, the sigma point which advanced time is given by Formula (17). This calculation is executed by the state transition model unit 220.

Further, the average of the predicted states is given by the following equation (18). This calculation is executed in the average calculation unit 230.

The predicted error covariance is given by the following equation (19).

Next, the estimated observation value is given by the following equation (20). The calculation of γ _{k + 1} (i) is executed in the observation model unit 240, and the calculation of Expression (20) is executed in the average processing unit 250.

Here, the covariance of the residual is given by the following equation (21).

The cross-correlation matrix is given by equation (22).

Next, the state transition model used in the calculation of the state transition model unit 220 will be described. In the posture estimation model in the present embodiment, it is assumed that among the state variables constituting the state vector, other than the posture q does not change from the previous time. For this reason, the following equation (23) is established. However, since the next state vector is actually corrected by the Kalman gain and the observation residual, the value does not change at all but approaches the true value.

Regarding the rotation of the posture, the equation (24) is established from the rotational equation of motion.

However, I _{3 × 3} is a _{3 × 3} unit matrix, and the symbol [v ×] is defined by the expression (25) for the three-dimensional vector v = (v1, v2, v3).

Further, the calculation of Expression (23) is defined by Expression (26) and Expression (27).

The quaternion q representing the posture in the state vector must satisfy the normalization condition ?? q ?? = 1, but if the average of the sigma points is obtained, that condition is not satisfied. When any operation is performed on the quaternion q, such a problem can be solved by normalizing the result after the operation with the size of the vector itself.
In order to maintain the normalization condition more strictly, the posture of the state vector is limited to only the difference information from the previous time using MRPs (modified Rodrigues parameters), and the posture information outside the Kalman filter is obtained from the Kalman filter. Update may be performed based on the difference information. As a result, the entire system functions as a device for estimating the posture.

Next, the calculation of the observation model executed by the observation model unit 240 will be described. The estimated observation value for the angular velocity sensor is simply given by equation (28).

It is estimated that the geomagnetic vector in the coordinate system in the order of horizontal east, horizontal north, and vertical fixed to the ground is given by equation (29).

Accordingly, the measured value estimated for the magnetic sensor is expressed by Equation (30) using the matrix of Equation (5).

Assuming that the gravity vector in the coordinate system in the order of horizontal east, horizontal north, and vertical fixed on the ground is normalized by gravitational acceleration, equation (31) is established.

Equation (32) is obtained from Equation (31). The observation residual is given by equation (33).

As described above, according to the present embodiment, among the state variables of the state vector _xk , the state variables whose initial state cannot be predicted (in this example, the quaternion q of the posture) are set differently in advance. The time until the state vector converges can be greatly reduced.
In particular, in setting the quaternion q of the posture, the reference posture is determined, and this reference posture is rotated by an equal angle (for example, 90 degrees) in each of the three orthogonal axes, thereby determining the initial posture. That is, since the posture uniformly distributed in the space is adopted as the initial posture, the state vector can be converged at high speed regardless of the posture of the mobile phone 1.

<2. Second Embodiment>
In the first embodiment described above, it is assumed that the initial posture is unpredictable. On the other hand, the cellular phone 1 according to the second embodiment is different in that the initial state of the posture is predicted to some extent from a part of the observed values and the initial posture is set.
The mobile phone 1 according to the second embodiment differs from the first embodiment in the content of the posture estimation program executed by the CPU 10. FIG. 8 shows a functional block diagram of the posture estimation apparatus obtained by executing the program. The functional blocks in FIG. 8 are different from the functional blocks in the first embodiment shown in FIG. 3 in that the nonlinear Kalman filters KF1 to KF4 are used instead of the nonlinear Kalman filters KF1 to KF10, and the initial value generator 130A is replaced with an initial value. The point is that the value generation unit 130B is used.

Assuming that the mobile phone 1 is not vibrating or accelerating in the initial state (time k = 0), the acceleration sensor detects only gravitational acceleration. The initial value generation unit 130B uses the value to determine a candidate for the initial value of the state vector. Using the observed value a ₀ = (a _0,1 , a _0,2 , a _0,3 ) of the acceleration sensor, the following is determined. As shown in FIG. 9, the angle φ in the equation (34) represents the angle of the gravitational acceleration vector with respect to the XY plane, and φ = 0 when the gravitational acceleration vector is downward on the z axis. Β in Expression (35) indicates the magnitude of the gravitational acceleration vector on the XY plane.

The p _a is defined by Equation (36) and (37).

When the pa determined in this way is regarded as a quaternion representing _a posture, only the inclination is accurate. Since it is uncertain to which direction it is tilted, a plurality of initial values are set with the same tilt but different tilt directions. Specifically, it is preferable to provide four initial postures symmetrical about the vertical axis.
In order to specify the four initial postures, first, four quaternions p ₀ , p ₁ , p ₂ , and p ₃ determined by the equations (38) to (41) are assumed.
p ₀ = (0, 0, 0, 1) ...... Formula (38)
_{p 1 = (0, 0,1 /} 2 1/2, 1/2 1/2) ...... equation (39)
p ₂ = (0, 0, 1, 0) (Equation 40)
p ₃ = (0, 0, −1/2 ^1/2 , 1/2 ^1/2 ) …… Equation (41)

Then, when the quaternion product, to calculate the _{p a (×) p 0,} p a (×) p 1, p a (×) p 2, p a (×) p 3, these (×) The initial posture is assumed. This calculation is executed by the initial value generation unit 130B to generate initial values INI1 to INI4. That is, the initial value generation unit 130B indicates a plurality of postures that are rotated by an equal angle around an axis perpendicular to the horizontal plane that matches the inclination of the mobile phone 1 with respect to the horizontal plane, based on the observation value of the acceleration sensor. Thus, a plurality of initial values INI1 to INI4 are generated.

  As described above, in the second embodiment, when the initial value of the state vector is set, the range of the initial value is limited using the output of the sensor for obtaining the observation value, and a plurality of initial values are included in the limited range. A value was set. For this reason, the number of nonlinear Kalman filters operated in parallel can be reduced, and the processing load on the CPU 10 can be reduced. Further, in the limited range, when ten initial values are used as in the first embodiment, the state vector can be converged at higher speed.

<3. Modification>
The present invention is not limited to the above-described embodiments, and for example, various modifications described below are possible.
(1) In the first and second embodiments described above, a sigma point Kalman filter, which is a kind of nonlinear Kalman filter, has been described as an example. However, the present invention is not limited to this, and any Kalman filter such as an extended Kalman filter can be used. You may apply to. Even when applied to the extended Kalman filter, the state vector can be converged in a short time.

(2) In the first and second embodiments described above, the Kalman filter is applied to posture estimation. However, in the present invention, the estimation target is not limited to the posture, and what is estimated It may be a target. In short, the present invention is applicable to all devices that estimate a state vector using a Kalman filter.

(3) In the first and second embodiments described above, the state vector set as the initial value includes a plurality of state variables, and by setting different values (attitudes) for some of them, different state vectors are set. However, the present invention is not limited to this. That is, different values may be set for all of the state variables of the state vector set as initial values.

(4) In the first embodiment described above, a certain posture of an object is set as a reference posture, and a posture obtained by rotating the reference posture by 90 degrees around each of the orthogonal XYZ axes is set as an initial posture. The initial posture is not limited, and a posture obtained by rotating the reference posture by an equal angle around each of three or more axes that intersect at an equal angle may be used as the initial posture. In short, it is desirable that a plurality of initial postures corresponding to a plurality of initial values are arranged evenly distributed in space.

(5) In the first and second embodiments described above, based on the components of the three-axis geomagnetic sensor is a part of the observation residuals e _k, but to determine the choice of non-linear Kalman filter KF, the present invention Is not limited to this. When a plurality of types of sensors are provided and observation values are generated for a plurality of observation targets, the observation residual _ek is composed of a plurality of observation target elements, and which nonlinear Kalman filter KF is selected based on a part thereof. Alternatively, it may be determined which nonlinear Kalman filter KF is to be selected based on all of them.

(6) In the second embodiment described above, a plurality of initial values are generated based on the output of the acceleration sensor that is a part of the observed value, but the present invention is not limited to this, A plurality of initial values may be generated using all of them.

  KF1 to KF10: nonlinear Kalman filter, 110: comparison unit, 120: selection unit, 130A, 130B ... initial value generation unit, PM1 to PM10 ... observation residual monitor, 40A, 40B ... selection signal generation circuit, INI1 to INI10 ... initial value , 210 ... sigma point generation unit, 220 ... state transition model unit, 230 ... average calculation unit, 240 ... observation model unit, 250 ... average processing unit, 270 ... Kalman gain applying unit.

Claims (11)

A plurality of sensors for observing the system and outputting observation values,
A state vector having a plurality of state variables indicating the state of the system as an element is estimated using a state transition model, an estimated observation value is calculated from the state vector estimated using an observation value model, and the estimated observation value and A plurality of Kalman filters for performing an operation of calculating the difference between the observation values of the plurality of sensors as an observation residual, and updating the state vector based on the observation residual and the estimated state vector;
An initial value generating unit that generates a plurality of initial values different from each other as an initial state of the state vector, and supplies the initial value to each of the plurality of Kalman filters;
Based on an observation residual relating to at least one of the plurality of sensors included in the observation residual, an output that identifies a Kalman filter that converges the state vector earliest and outputs the state vector of the identified Kalman filter With
The plurality of Kalman filters operate in parallel to calculate the observation residuals simultaneously,
The output unit calculates, for each of the plurality of Kalman filters, a maximum value of an observation residual relating to the at least one sensor in a predetermined period from a current time to a past time by a predetermined time, and calculates the plurality of calculated maximum values. A Kalman filter corresponding to the smallest maximum value among them is specified as a Kalman filter in which the state vector converges fastest,
The state estimation apparatus characterized by the above-mentioned.   The initial value generation unit includes a storage unit that stores the plurality of initial values, reads the plurality of initial values from the storage unit, and supplies the plurality of initial values to each of the plurality of Kalman filters. The state estimation apparatus described in 1. The state vector of the system includes a state variable indicating the posture of the object,
The plurality of initial values are a plurality of initial postures obtained by rotating the reference posture by an equal angle around each of three or more axes intersecting with the reference posture of the object at the same angle.
The state estimation apparatus according to claim 2. A plurality of sensors for observing the system and outputting observation values,
A state vector having a plurality of state variables indicating the state of the system as an element is estimated using a state transition model, an estimated observation value is calculated from the state vector estimated using an observation value model, and the estimated observation value and A plurality of Kalman filters for performing an operation of calculating the difference between the observation values of the plurality of sensors as an observation residual, and updating the state vector based on the observation residual and the estimated state vector;
An initial value generating unit that generates a plurality of initial values different from each other as an initial state of the state vector, and supplies the initial value to each of the plurality of Kalman filters;
Based on an observation residual relating to at least one of the plurality of sensors included in the observation residual, an output that identifies a Kalman filter that converges the state vector earliest and outputs the state vector of the identified Kalman filter With
The plurality of Kalman filters operate in parallel to calculate the observation residuals simultaneously,
The initial value generation unit generates the plurality of initial values based on the observation values output from some or all of the plurality of sensors.
The state estimation apparatus characterized by the above-mentioned. The output unit calculates, for each of the plurality of Kalman filters, a maximum value of an observation residual relating to the at least one sensor in a predetermined period from a current time to a past time by a predetermined time, and calculates the plurality of calculated maximum values. A Kalman filter corresponding to the smallest maximum value among them is specified as a Kalman filter in which the state vector converges fastest,
The state estimation apparatus according to claim 4. The state vector of the system includes a state variable indicating the posture of the object,
The plurality of sensors includes an acceleration sensor that detects acceleration of the object in three orthogonal axes,
The initial value generation unit indicates each of a plurality of postures that are rotated by an equal angle around an axis perpendicular to the horizontal plane that coincides with the inclination of the object with respect to the horizontal plane based on the observation value of the acceleration sensor. Generating the plurality of initial values,
The state estimation apparatus according to claim 4 or 5, wherein The plurality of sensors include predetermined sensors that observe the system in three orthogonal axes,
The observation residual is composed of a plurality of elements including specific elements corresponding to the observation values of the predetermined sensor,
The output unit calculates a maximum value of the specific element of the observation residual in the predetermined period for each of the plurality of Kalman filters, a Kalman filter corresponding to the smallest maximum value among the plurality of calculated maximum values, Identify the Kalman filter that the state vector converges fastest,
The state estimation apparatus according to any one of claims 1 to 6, wherein A state estimation method for estimating a state of the system based on observation values output from a plurality of sensors observing the system,
A plurality of initial values different from each other as an initial state of a state vector having a plurality of state variables indicating the state of the system as elements,
The state vector is estimated using a state transition model, an estimated observation value is calculated from the state vector estimated using an observation value model, and the difference between the estimated observation value and the observation values of the plurality of sensors A Kalman filtering process that calculates the difference and updates the state vector based on the observed residual and the estimated state vector is performed in parallel for each of the plurality of initial values;
A maximum value for an observation residual relating to at least one of the plurality of sensors included in the observation residual in a predetermined period from the current time to a past time is calculated for each of the plurality of Kalman filter processes. And
A Kalman filter process corresponding to the smallest maximum value among the maximum values calculated for each of the plurality of Kalman filter processes is identified as a Kalman filter process in which the state vector converges fastest,
Output the identified Kalman filter state vector,
A state estimation method characterized by the above. A state estimation program used in a state estimation device that estimates a state of the system based on observation values output from a plurality of sensors that observe the system,
A process of generating a plurality of different initial values as an initial state of a state vector having a plurality of state variables indicating the state of the system as elements;
The state vector is estimated using a state transition model, an estimated observation value is calculated from the state vector estimated using an observation value model, and the difference between the estimated observation value and the observation values of the plurality of sensors A Kalman filter process that calculates the difference and updates the state vector based on the observed residual and the estimated state vector, a process of executing in parallel for each of the plurality of initial values;
A maximum value for an observation residual relating to at least one of the plurality of sensors included in the observation residual in a predetermined period from the current time to a past time is calculated for each of the plurality of Kalman filter processes. Processing to
A process for identifying a Kalman filter process corresponding to the smallest maximum value among the maximum values calculated for each of a plurality of the Kalman filter processes as a Kalman filter process in which the state vector converges most quickly;
A process of outputting the identified Kalman filter state vector;
A state estimation program to be executed by a computer. A state estimation method for estimating a state of the system based on observation values output from a plurality of sensors observing the system,
Based on the observation values output from some or all of the plurality of sensors, generate a plurality of initial values different from each other as an initial state of a state vector having a plurality of state variables indicating the state of the system,
The state vector is estimated using a state transition model, an estimated observation value is calculated from the state vector estimated using an observation value model, and the difference between the estimated observation value and the observation values of the plurality of sensors A Kalman filtering process that calculates the difference and updates the state vector based on the observed residual and the estimated state vector is performed in parallel for each of the plurality of initial values;
A maximum value for an observation residual relating to at least one of the plurality of sensors included in the observation residual is calculated for each of the plurality of Kalman filter processes;
A Kalman filter process corresponding to the smallest maximum value among the maximum values calculated for each of the plurality of Kalman filter processes is identified as a Kalman filter process in which the state vector converges fastest,
Output the identified Kalman filter state vector,
A state estimation method characterized by the above. A state estimation program used in a state estimation device that estimates a state of the system based on observation values output from a plurality of sensors that observe the system,
A process of generating a plurality of initial values different from each other as an initial state of a state vector having a plurality of state variables indicating the state of the system as elements based on the observation values output from some or all of the plurality of sensors When,
The state vector is estimated using a state transition model, an estimated observation value is calculated from the state vector estimated using an observation value model, and the difference between the estimated observation value and the observation values of the plurality of sensors A Kalman filter process that calculates the difference and updates the state vector based on the observed residual and the estimated state vector, a process of executing in parallel for each of the plurality of initial values;
A process of calculating a maximum value for an observation residual associated with at least one of the plurality of sensors included in the observation residual for each of the plurality of Kalman filter processes;
A process for identifying a Kalman filter process corresponding to the smallest maximum value among the maximum values calculated for each of a plurality of the Kalman filter processes as a Kalman filter process in which the state vector converges most quickly;
A process of outputting the identified Kalman filter state vector;
A state estimation program to be executed by a computer.

Images