JP2012237704A - Terrestrial magnetism measuring device, terrestrial magnetism measuring method and terrestrial magnetism measurement program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To detect a precise value of terrestrial magnetism.SOLUTION: A mobile phone 1 has a ROM30 which stores a terrestrial magnetism measurement program 100 having an initial value generation module 110 and a nonlinear Kalman filter module 120, a terrestrial magnetism detection part 70, an acceleration detection part 80 and an angular velocity detection part 90. The nonlinear Kalman filter module 120 executes a calculation of the nonlinear Kalman filter KF and estimates a state vector xwhich includes a first variable ρ for expressing an intensity of terrestrial magnetism r and a second variable η for expressing an inclination of terrestrial magnetism θ as elements.

Description

  The present invention relates to a geomagnetism measuring apparatus, a geomagnetism measuring method, and a geomagnetism measuring program.

  When calculating the magnitude and direction of geomagnetism, it is possible to integrate different sensor outputs such as angular velocity sensors and acceleration sensors in addition to the output of the geomagnetic sensor, so that the output of the geomagnetic sensor includes noise. It is expected to calculate an accurate value at high speed.

  Nonlinear Kalman filters such as an extended Kalman filter (EKF) and a sigma point Kalman filter (SPKF) are known as methods for integrating the outputs of a plurality of sensors that measure different physical quantities and estimating the state of a dynamic system. For example, 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.

Japanese Patent Laid-Open No. 9-5104

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

In general, a nonlinear Kalman filter is composed of a state transition model that estimates changes over time in a plurality of physical quantities representing the state of a dynamic system, and a plurality of sensors that the dynamic system has based on the estimated state of the dynamic system. An observation model for estimating a value to be measured (observation value). The nonlinear Kalman filter uses a difference (observation residual) between the estimated observation value and the observation value actually measured by a plurality of sensors, and uses a plurality of physical quantities representing the state of the dynamic system as elements. Update the vector to a value closer to the true value.
However, when the calculation using a nonlinear Kalman filter is repeated, the state of the dynamic system is changed from the actual value due to the influence of noise superimposed on the observation value, etc. and the nonlinearity of the state transition model and the observation model. There is a problem that there is a case where the value is estimated as a greatly deviated value.

  The present invention has been made in view of the above-described points, and when a state of a dynamic system is estimated using a nonlinear Kalman filter, an estimated value is an inappropriate value greatly deviating from an actual value. It is an object to solve this problem and to calculate an accurate value of geomagnetism.

  In order to solve the above problems, a geomagnetism measuring apparatus according to the present invention includes a plurality of sensors for observing a system and outputting observation values, and a plurality of state variables indicating a state of the system using a state transition model. A state vector is estimated, an estimated observation value is calculated from the state vector using an observation value model, a difference between the estimated observation value and the observation values of the plurality of sensors is calculated as an observation residual, and the observation A Kalman filter that executes an operation of updating the state vector based on a residual and the state vector, and represents the strength of the geomagnetism using the first variable and takes a value larger than the first value. When the function is the first function, the second variable is used to represent the dip of geomagnetism, and the function that takes a value larger than the second value and smaller than the third value is the second function, First variable and previous A second variable, characterized in that it is a component of the state vector.

According to the present invention, by calculating the Kalman filter, a variable of the first function (first variable) representing the strength of geomagnetism and a variable of the second function (second variable) representing the dip angle of geomagnetism are included as elements. Update the state vector with the observation residuals to bring it closer to the true value.
When performing an operation using the Kalman filter, the state variable constituting the calculated state vector may be calculated as a value greatly deviating from the true value due to the influence of noise superimposed on the observed value. If the state variable is once calculated as a value greatly deviating from the true value, there is a high possibility that the state variable will not approach the true value even if the calculation by the Kalman filter is repeated thereafter.
The strength of the geomagnetism has a value greater than a certain value at any position on the earth. Therefore, it can be said that when the strength of the geomagnetism is less than or equal to the certain value, it is a value greatly deviating from the true value. Therefore, when the magnetic strength is calculated as a value less than or equal to the certain value by the Kalman filter calculation, the magnetic strength is restored to a correct value that is greater than or equal to the certain value even if the Kalman filter calculation is repeated thereafter. It is unlikely to be done.
According to the present invention, the strength of geomagnetism is represented by a first function that is larger than the first value. Therefore, no matter what value the first variable changes as a result of the calculation using the nonlinear Kalman filter, the strength of the geomagnetism is calculated as a value less than or equal to the first value that is significantly different from the true value. There is no. Thereby, it is possible to prevent the geomagnetism intensity from being calculated as a value greatly deviating from the true value, and to prevent an inaccurate geomagnetism value from being calculated.

Further, the geomagnetism is a magnetic field having a horizontal component toward the north of the magnetic pole and a vertical component in the dip direction, and the dip angle of the geomagnetism takes a value within a certain range no matter where the earth is observed. Therefore, when the geomagnetic dip angle is calculated as a value exceeding the certain range by the Kalman filter operation, the magnetic dip angle is restored to the correct value within the certain range even if the Kalman filter operation is repeated thereafter. Is unlikely.
According to the present invention, the dip angle of geomagnetism is represented by a second function that takes a value larger than the second value and smaller than the third value. Therefore, no matter what value the second variable changes as a result of the calculation using the nonlinear Kalman filter, the geomagnetic dip angle is smaller than the second value greatly deviating from the true value, or the third It is not calculated as a value larger than the value of. Thereby, it is possible to prevent the dip angle of geomagnetism from being calculated as a value greatly deviating from the true value, and as a result, it is possible to prevent calculating an incorrect geomagnetic value.

In the geomagnetic measurement apparatus described above, when the first variable is ρ and the second variable is η, the first function is expressed by exp (ρ), and the second function is tan ^−1. It is preferable to be represented by η.

According to the present invention, since the strength of geomagnetism is expressed as the first function exp (ρ) using the first variable ρ, exp (ρ)> 0 is satisfied regardless of the value of the variable ρ. .
The strength of geomagnetism needs to be a positive value. Therefore, it can be said that when the strength of the geomagnetism is a negative value, the value is greatly deviated from the true value. Therefore, when the magnetic strength is calculated as a negative value by the calculation of the Kalman filter, the possibility that the magnetic strength is restored to a positive value is low even if the calculation by the Kalman filter is repeated thereafter.
According to the present invention, since the strength of geomagnetism is expressed by the first function exp (ρ) having a value larger than “0”, the value of the first variable ρ varies as a result of the calculation using the Kalman filter. Even so, the strength of the geomagnetism is not calculated as a value of “0” or less that is significantly different from the true value. Thereby, it is possible to prevent the geomagnetism intensity from being calculated as a value greatly deviating from the true value, and to prevent an inaccurate geomagnetism value from being calculated.

Similarly, according to the present invention, since the dip angle of the geomagnetism is expressed as the second function tan ⁻¹ η using the second variable η, even if the variable η changes to any value, −π / 2 <tan ^{− 1} η <π / 2 is satisfied.
The geomagnetism is a magnetic field having a horizontal component toward the north of the magnetic pole and a vertical component in the direction of the dip. The dip of the geomagnetism approaches +90 degrees as it approaches the north magnetic pole, and decreases to -90 degrees as it approaches the south magnetic pole. Get closer. That is, the dip angle of geomagnetism needs to be larger than −π / 2 and smaller than π / 2. Therefore, if the geomagnetic dip angle is calculated as a value of −π / 2 or less or a value of π / 2 or more by the calculation of the Kalman filter, the magnetic dip angle is reduced even if the calculation by the Kalman filter is repeated thereafter. It is unlikely to be restored to a correct value that is greater than -π / 2 and smaller than π / 2.
According to the present invention, the dip angle of the geomagnetism is expressed by the second function tan ⁻¹ η that takes a value larger than −π / 2 and smaller than π / 2. Regardless of the value of η, the dip angle of geomagnetism is not calculated as a value of −π / 2 or less, or a value of π / 2 or more greatly deviating from the true value. Thereby, it is possible to prevent the dip angle of geomagnetism from being calculated as a value greatly deviating from the true value, and as a result, it is possible to prevent calculating an incorrect geomagnetic value.

Next, a geomagnetism measurement method according to the present invention is a state estimation method used in a state estimation device that includes a plurality of sensors that observe a system and output observation values, respectively, and uses the state transition model of the system. A state vector including a plurality of state variables indicating states is estimated, an estimated observation value is calculated from the state vector using an observation value model, and a difference between the estimated observation value and the observation values of the plurality of sensors is calculated. Calculated as an observation residual, updates the state vector based on the observation residual and the state vector, represents the strength of geomagnetism using the first variable, and takes a value larger than the first value When the function is the first function, the second variable is used to represent the dip of geomagnetism, and the function that takes a value larger than the second value and smaller than the third value is the second function, First variable and second variable But it characterized in that it is a component of the state vector.

  According to the present invention, it is possible to prevent the strength and the dip angle of the geomagnetism from being calculated as values greatly deviating from the true values, and to prevent the calculation of inaccurate geomagnetism values.

Next, a geomagnetism measurement program used in a geomagnetism measurement apparatus according to the present invention is a geomagnetism measurement program used in a geomagnetism measurement apparatus including a plurality of sensors that observe a system and output observation values, respectively, and a state transition model A process for estimating a state vector having a plurality of state variables indicating the state of the system as elements, a process for calculating an estimated observation value from the state vector using an observation value model, the estimated observation value, and the A process for calculating a difference from observation values of a plurality of sensors as an observation residual and a process for updating the state vector based on the observation residual and the state vector are executed by a computer, and the first variable is set to The function that represents the strength of the geomagnetism, the function that takes a value larger than the first value as the first function, and the second variable is used to represent the dip angle of the geomagnetism. A function that takes a larger and smaller than the third value than the value, when the second function, and the first variable and the second variable, characterized in that it is a component of the state vector.

  According to the present invention, it is possible to prevent the strength and the dip angle of the geomagnetism from being calculated as values greatly deviating from the true values, and to prevent the calculation of inaccurate geomagnetism values.

It is a block diagram which shows the structure of the mobile telephone which concerns on embodiment. It is a perspective view which shows the external appearance of a mobile telephone. It is a functional block diagram of a nonlinear Kalman filter. It is explanatory drawing for demonstrating a mode vector converging.

<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 phone according to an embodiment of the present invention, and FIG. 2 is a perspective view showing an 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 Kalman filter based on the outputs of various sensors and estimating the attitude of the mobile phone 1.

  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 (magnetic impedance 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 is data indicating a vector having a horizontal component toward the magnetic pole north and a vertical component in the dip direction by three components of the x-axis, y-axis, and z-axis. In addition, since the 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 outside the mobile phone 1, and the actual geomagnetism Detects the combined magnetic field. However, in the state transition model assumed in the geomagnetism measurement program 100 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 is data indicating the resultant force of inertial force and gravity in an object-fixed system that moves integrally with the acceleration detector 80 as a vector having three components of the x-axis, y-axis, and z-axis. 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 estimates a plurality of physical quantities representing the state of the dynamic system, for example, the posture of the mobile phone 1, the strength of the geomagnetism, the depression angle, and the like by executing the geomagnetism measurement program 100 stored in the ROM 30. That is, the mobile phone 1 functions as a geomagnetism measuring device that calculates an accurate value of geomagnetism based on the estimated values of a plurality of physical quantities representing the state of the dynamic system.
The geomagnetism measurement program 100 includes an initial value generation module 110 and a nonlinear Kalman filter module 120. The nonlinear Kalman filter module 120 performs an operation of the nonlinear Kalman filter KF.

The nonlinear Kalman filter is a state transition model that estimates changes over time in a plurality of physical quantities representing the state of a dynamic system, an observation model that estimates values (observation values) measured by a plurality of sensors in the dynamic system, Have
The nonlinear Kalman filter estimated the state vector after a unit time from a state vector whose elements are a plurality of physical quantities representing the state of the dynamic system at a certain time, using the state transition model. An observation value vector having a plurality of observation values as elements is estimated based on the state vector. Here, the observed value vector includes the value of the geomagnetic data output from the geomagnetic detection unit 70, the value of the acceleration data output from the acceleration detection unit 80, and the value of the angular velocity data output from the angular velocity detection unit 90. Is a vector.
Next, the nonlinear Kalman filter is an observation calculated as a difference between the estimated observation value vector and an observation value vector whose elements are actual outputs from the geomagnetic detection unit 70, the acceleration detection unit 80, and the angular velocity detection unit 90. Based on the residual, the state vector is updated to a value closer to the true value. The nonlinear Kalman filter updates the estimated values of a plurality of physical quantities representing the state of the dynamic system to an accurate value closer to the actual value (true value) by repeating the above operation.
The nonlinear Kalman filter in the present embodiment is a quaternion q representing the attitude of the mobile phone 1, the geomagnetic strength r, the geomagnetic depression angle θ, the angular velocity ω of the mobile phone 1, the angular velocity sensors 91 to 91 as physical quantities representing the state of the dynamic system. The offset estimated value g _{o of} 93 and the offset estimated values m _o of the geomagnetic sensors 71 to 73 are adopted, and these are used as estimation targets.

By the way, when the calculation using the nonlinear Kalman filter is repeated, the estimated physical quantity greatly deviates from the actual value due to the influence of noise superimposed on the observation value and the nonlinearity of the state transition model and the observation model. It may be calculated as a value. In this case, it is difficult to estimate a physical quantity close to the true value even if the calculation by the nonlinear Kalman filter is repeated thereafter.
Therefore, in the present embodiment, among a plurality of physical quantities representing the state of the dynamic system, a physical quantity that can define a value range is represented using a state variable that does not change to a value outside the range. Then, determine the state vector x _k such that a state variable corresponding to each of a plurality of physical quantity as elements, performing the calculation of the Kalman filter. In the following, state variables according to the present embodiment will be described.

First, among the plurality of physical quantities representing the state of the dynamic system, the geomagnetic strength r and the geomagnetic dip angle θ are examined.
As described above, the geomagnetism is a magnetic field having a horizontal component toward the magnetic pole north and a vertical component in the dip direction. Specifically, the dip angle of geomagnetism approaches +90 degrees (third value) as it approaches the north magnetic pole, and approaches -90 degrees (second value) as it approaches the south magnetic pole. That is, the dip angle θ of geomagnetism satisfies −π / 2 <θ <π / 2. Further, the strength r of geomagnetism is a value larger than 0 (first value).
Therefore, in this embodiment, the strength r of geomagnetism is expressed as a function (first function) exp (ρ) shown in Expression (1) using a variable ρ, and this variable ρ is a state variable. Further, the dip angle θ of geomagnetism is expressed as a function (second function) tan ⁻¹ η shown in Expression (2) using a variable η, and this variable η is a state variable.
In this case, exp (ρ)> 0 is satisfied regardless of the value of the variable ρ. That is, even when the calculation of the nonlinear Kalman filter is repeated, the geomagnetic strength r satisfies the condition r> 0. Moreover, even if the variable η changes to any value, −π / 2 <tan ⁻¹ η <π / 2 is satisfied. That is, even when the calculation of the nonlinear Kalman filter is repeated, the geomagnetic dip angle θ satisfies the condition of −π / 2 <θ <π / 2.

The quaternion q representing the posture of the mobile phone 1 is a mathematical expression representing the posture (rotation state) of the object, and is given as a four-dimensional number. Here, the 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 (3).
Since the mobile phone 1 can take any posture, the range of the value of the quaternion q representing the posture cannot be limited. Therefore, the quaternion q representing the attitude is directly adopted as a state variable without limiting the value.

Also, the angular velocity ω, the estimated offset value g _o of the angular velocity sensor, and the estimated offset value m _o of the geomagnetic sensor can take arbitrary values, and thus are directly adopted as state variables without limiting the values.
The state variable includes the estimated offset value m _o of the geomagnetic sensor and the estimated offset value g _o of the angular velocity sensor because the offset of the sensors 71 to 73 is superimposed on the geomagnetic data, and the sensors 91 to 73 are superimposed on the angular velocity data. This is because 93 offsets are superimposed. 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 accurately estimate a plurality of physical quantity representing the state of a dynamic system, it is necessary to use the angular velocity data corrected by the corrected geomagnetic data and offset estimate g _o by offset estimate m _o.

The state vector x _{k at} time k having the state variables defined as described above as elements is expressed by the following equation (4).

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 observation value vectors y each composed of three elements of the X axis, the Y axis, and the Z axis. Observation value vector y _{k at} time k is given by equation (5).

The nonlinear Kalman filter KF estimates a plurality of physical quantities representing the state of the dynamic system using the state vector x _k and the observed value vector y _k . Specifically, the nonlinear Kalman filter KF uses a state transition model that represents a change in the state of the dynamic system over time, from a state vector x _k−1 that indicates the state of the system at the current time (time k−1). The state vector x _k after the unit time has elapsed (time k) is estimated. Next, an observation value vector y _k is estimated from the estimated state vector x _k using an observation model that estimates the observation values of various sensors from the state of the dynamic system. Then, the estimated observed value vector (estimated observed values), calculates the difference between the observed value vector y _k representing the output of various sensors (the actual observed value) as the observation residuals e _k, observation residuals e _k Kalman gain _Kk is calculated based on Based on the observation residual e _k calculated in this way and the Kalman gain K _k , the nonlinear Kalman filter KF updates the state vector x _k to a more accurate value.

The initial value generation module 110 calculates an initial value INI (state vector) having the state variable at time k = 0 as an element, and outputs it to the nonlinear Kalman filter module 120. The initial value INI, the initial value q ₀ of the quaternion q representing the _orientation, initial value ρ ₀ of the variable _ρ, initial value η ₀ of the variable _η, the initial value ω ₀ of the angular velocity _ω, the initial offset the estimated value g _o of the angular velocity sensor The value g _{o, 0} and the initial value m _{o, 0} of the offset estimation value m _o of the geomagnetic sensor are included.
The initial value INI may be set as appropriate so that the state variable converges to an accurate value as soon as possible. For example, the initial value INI may be set as described below.

The initial value ρ ₀ of the variable ρ for calculating the strength r of geomagnetism and the initial value η ₀ of the variable η for calculating the dip angle θ of geomagnetism are based on, for example, position information supplied from the GPS unit 60. To generate. 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 initial value generation module 110 stores a lookup table LUT1 in which the position information, the geomagnetic strength r, and the dip angle θ are stored in the ROM 30 in association with each other, and the position information is referred to by referring to this. Read the corresponding geomagnetic strength r and dip angle θ. Then, the initial value generation module 110 calculates an initial value ρ ₀ based on the geomagnetic strength r corresponding to the position information, and calculates an initial value η ₀ based on the dip angle θ corresponding to the position information. 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 module 110 reads a typical value from the lookup table LUT1. For example, for the mobile phone 1 sold in Japan, the initial value ρ ₀ and the initial value η ₀ may be calculated based on the strength r and the dip angle θ of Akashi City.

The initial value ω ₀ of the initial value of the angular velocity ω is set to “0” on the assumption that the mobile phone 1 is stationary, for example. Offset estimate value g of the angular velocity sensor. The initial value g _{o, 0} also, for example, assuming that the portable telephone 1 is stationary is set to "0". For the initial value q ₀ of quaternion q representing the orientation, for example, assuming that the portable telephone 1 is stationary facing in a certain direction, a value such as to reduce the deviation between the actual initial position, initial Set to the value INI.

The initial value m _{o, 0} of the estimated offset value m _o of the geomagnetic sensor is, for example, the observed value m ₀ of the geomagnetic sensor at time k = 0, the geomagnetic vector m _{typical of the} area where the mobile phone 1 is used, and the formula (7 ) Is set to a value given by the following equation (6) using the matrix B (q) shown in FIG. Here, the matrix B (q) is a matrix used for converting a coordinate system fixed to the ground into a coordinate system fixed to the geomagnetic sensor of the mobile phone 1 using the posture q, and is shown below. It is given by equation (7). The ROM 30 stores a lookup table LUT2 that stores positional information (longitude, latitude) and a geomagnetic vector m _typical in association with each other. The initial value generation module 110 obtains the geomagnetic vector m _typical by referring to the lookup table LUT2 based on the position information generated by the GPS unit 60, and calculates the offset of the geomagnetic sensor by calculating Expression (6). obtain an initial value _{m o, 0} of the value _{m o.}

  As described above, the initial value generation module 110 outputs the initial value INI to the nonlinear Kalman filter KF.

[2. Nonlinear Kalman filter]
Next, the content of the nonlinear Kalman filter KF will be described.

[2.1. Overview of nonlinear Kalman filter]
FIG. 3 is a functional block diagram of the nonlinear Kalman filter KF. The state transition model of the system of the nonlinear Kalman filter KF is given by the following equation (8), and the observation model is given by the following equation (9).

Here, the state vector x _k appearing in Equation (8) is an n-dimensional vector, and the observed value vector y _k appearing in Equation (9) is an m-dimensional vector (in this embodiment, n = 15, m = 9). As shown in the equation (4), the state vector x _k includes state variables such as the quaternion q _k representing the posture and the estimated offset value m _{o, k of the} geomagnetic sensor. The process noise w _k and the observation noise υ _k are Gaussian noises centered on zero.
Equation (8) represents a state transition model of the nonlinear Kalman filter KF. Equation (8), the state vector _{x k} at time k is, by substituting the state vector _{x k-1} at time k-1 to function f, adding the process noise _{w k-1} at the result and time k-1 It is shown that
Equation (9) represents an observation model of the nonlinear Kalman filter KF. Equation (9) indicates that the observed value vector y _k at time k is obtained by substituting the state vector x _k at time _k into the function h and adding the result to the observed noise υ _{k at} time k. ing. In the following description, the covariance of the process noise w _k is Q _k , and the covariance of the observation noise υ _k is R _k .

The observation residual e _k is a difference between the observation value vector y _{k at} time k and the observation value vector estimated using the observation model, and can be expressed by the following equation (10).
Further, the nonlinear Kalman filter updates the state vector estimation value as shown in the following equation (12) using the observation residual e _k and the Kalman gain K _k shown in the equation (11). As shown in Expression (13), the state vector estimation error covariance is updated.
As shown in Expression (10), the observed value vector estimated using the observation model is calculated by applying the estimated value of the state vector x _k to the observation model. Thus, the observed value vector y _k, the observation residuals e _k is the difference between the observed value vector estimated by using the observation model, or the state vector x _k has converged to an appropriate value close to the actual value not Can be determined.

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

[2.2. Calculation with Sigma Point Kalman Filter]
In the present embodiment, the nonlinear Kalman filter KF is configured by a sigma point Kalman filter using unscented transformation.
Hereinafter, update of the state vector, calculation of the state vector estimation error, calculation of the observation vector estimation value, and calculation of the cross-correlation matrix when the sigma point Kalman filter is used will be described in detail.

In the sigma point Kalman filter, first, (2n + 1) sigma points are generated using a covariance matrix of n rows and n columns. First, equations (14) and (15) are defined using a scaling parameter λ representing the spread of sigma points around the average of a plurality of sigma points.

At this time, the sigma point is determined by Expression (16) and Expression (17). The calculations of Expressions (14) to (17) are executed by the sigma point generation unit 210. The sigma point generator 210 is supplied with an initial value INI. In the first calculation of the sigma point Kalman filter, the sigma point generation unit 210 generates the sigma point using the initial value INI as the right side of the equation (16).

The state transition model unit 220 estimates a sigma point at time k after a unit time has elapsed from a sigma point at a certain time k-1. This calculation is given by equation (18).

Moreover, the average of the estimated state vector is given by the following equation (19). This calculation is executed in the average calculation unit 230.

The estimated state vector error covariance is given by the following equation (20).

Next, the estimated observation value is given by the following equation (21). The calculation of γ _k (i) is executed in the observation model unit 240, and the calculation of Expression (21) is executed in the average processing unit 250.

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

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

[2.3. State transition model]
Next, the state transition model used in the calculation of the state transition model unit 220 will be described.
The orientation estimation model in the present embodiment, as in equation (24) shown below, among the state variables that constitute the state vector x _k, it is assumed that except quaternion q representing the orientation does not change from the previous time. However, since the next state vector x _k is actually corrected by the Kalman gain K _k and the observation residual e _k , the value does not change at all and approaches the true value.

Here, the operator Ω representing the rotation of the posture is defined by the following equation (25).
However, I _{3 × 3} is a _{3 × 3} unit matrix, and the symbol [u ×] is defined by the equation (26) for the three-dimensional vector u = (u1, u2, u3). Further, with ΔT as a measurement time interval, a component constituting the operator Ω is defined by Expression (27).

At this time, the calculation for estimating the quaternion q _k representing the posture at time k after the unit time has elapsed from the quaternion q _k−1 representing the posture at a certain time k−1 is expressed by Expression (28).

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

[2.4. Observation model]
Next, the calculation of the observation model executed by the observation model unit 240 will be described.
Estimate gamma _Gyro observations g is the output of the angular velocity sensor 91 to 93, by using the angular velocity omega, the angular velocity sensor and an offset estimation value _{g o,} given by equation (29).

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 (30). Therefore, the estimated value γ _mag of the observed value m, which is the output of the geomagnetic sensors 71 to 73, is given by Expression (31) using the matrix of Expression (7).

If the gravity vector in the coordinate system in the order of horizontal east, horizontal north, and vertical fixed to the ground is normalized by the gravitational acceleration, Expression (32) is established. Therefore, the estimated value γ _acc of the observed value a which is the output of the acceleration sensors 81 to 83 is given by Expression (33) using the matrix of Expression (7).

Therefore, the observation residual _ek is expressed by the equation (34) by applying the equations (5), (29), (31), and (33) to the equation (10).

[3. Convergence of state variables]
The nonlinear Kalman filter KF according to the present embodiment described above does not directly adopt a plurality of physical quantities representing the state of the dynamic system as the state vector x _k , but for physical quantities that can limit the range of values, and the physical quantity is represented by the state variable that does not change to a value outside that range, and the elements of the state vector x _k.
Thereby, the nonlinear Kalman filter KF according to the present embodiment prevents the state vector x _k from converging to an inappropriate value, and estimates a value close to a true value for a plurality of physical quantities representing the state of the dynamic system. It is possible.

In the following, in order to show the effectiveness of the nonlinear Kalman filter KF according to the present embodiment, a case where a plurality of physical quantities representing the state of the dynamic system are employed as state variables as they are (hereinafter referred to as “comparative”). Will be described in comparison.
For the comparison, a state vector ^C x _k shown in the following equation (35) is used. The state vector ^C x _k includes, as its elements, the geomagnetic strength r instead of the variable ρ and the geomagnetic dip angle θ instead of the variable η, according to the present embodiment shown in the equation (4). It differs from the state vector _{x k.}

When the state vector ^C x _k is used for the nonlinear Kalman filter KF, the geomagnetism strength r and the geomagnetic depression angle θ may converge to values that are significantly different from actual ones.
As shown in FIG. 4, it is assumed that the actual value (true value) of the geomagnetic strength r is 0.5 gauss and the actual value (true value) of the geomagnetic dip angle θ is π / 3, Using a case where the mobile phone 1 is assumed to be in a reference posture, the state vector x _{k according} to this embodiment converges, and the state vector ^C x _k converges to a value far from the true value. Will be described.

When the geomagnetism strength r is expressed using the variable ρ as in this embodiment, the geomagnetism strength r does not deviate from the range that r can take (r> 0). Similarly, when the geomagnetic dip angle θ is expressed using the variable η, the geomagnetic dip angle θ does not deviate from a possible range of θ (−π / 2 <θ <π / 2). In this case, there is a high possibility that the geomagnetic strength r and the dip angle θ will converge to true values. In the example of FIG. 4, for example, in the state vector x _k , the geomagnetic strength r represented by the variable ρ converges to r = 0.5 gauss, and the geomagnetic depression angle θ represented by the variable η is There is a high possibility of convergence to θ = π / 3.

On the other hand, in the comparative example, there at the instant k = k _a, geomagnetic strength r and dip θ becomes larger away value from the true value, then it may be re-converges to a false value without approaching the true value. In the example of FIG. 4, for example, in the state vector ^C x _k , the geomagnetic strength r may converge to r = −0.5 gauss and the geomagnetic depression angle θ may converge to θ = −2π / 3. .

In this case, the observation residual when the state vector x _k converges to a true value and the observation residual when the state vector ^C x _k converges to a false value are equal. Specifically, the estimated value γ _{mag according} to the present embodiment shown in the equation (36) and the estimated value ^C γ _{mag according} to the _{proportionality} shown in the equation (37) are equal to each other. Therefore, the observation residuals of both are equal.

Therefore, when the estimated value γ _mag according to the present embodiment is a value close to the true value, the estimated value ^C γ _mag related to the _{proportionality} is also considered to be a value close to the true value, and therefore, The state vector ^C x _k is not corrected so as to approach the true value by the observation residual. That is, the geomagnetic strength r and the dip angle θ estimated by the nonlinear Kalman filter KF, which is proportional to each other, are false values that are significantly different from the true values, but are locally optimal solutions that are not corrected even by updating due to observation residuals. After that, there is no convergence (restoration) to the correct value.

As described above, in the present embodiment, among the physical quantities that represent the state of the dynamic system that is the estimation target of the nonlinear Kalman filter KF, the physical quantities that can determine the range of values are values other than the determined range. State variables were defined so as not to become.
Specifically, the geomagnetism strength r and the dip angle θ to be estimated by the nonlinear Kalman filter KF are expressed as a function exp (ρ) using the variable ρ and a function tan ⁻¹ η using the variable η. The strength r of the magnetic field does not deviate from the condition that r> 0, and the dip angle θ of the geomagnetism does not deviate from the condition of −π / 2 <θ <π / 2. Thus, the state vector x _k can be prevented from falling into a local optimal solution, the state vector x _k becomes possible to prevent the stabilized at a value far from the true value.

<B. 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 embodiment described above, a sigma point Kalman filter, which is a kind of nonlinear Kalman filter, has been described as an example. However, the present invention is not limited to this, and may be applied to any nonlinear Kalman filter such as an extended Kalman filter. For physical quantities that can determine the range of values among the physical quantities that represent the state of the dynamic system, by defining state variables that do not change the physical quantities to values outside the range, in calculations using nonlinear Kalman filters Thus, it is possible to prevent the state vector from falling into the local optimum solution.

(2) Modification 2
In the above-described embodiment, the state variable x _k is defined so as to limit the range of the values of the geomagnetism strength r and the dip angle θ among the plurality of physical quantities representing the state of the dynamic system. However, the present invention is not limited to this, and among physical quantities that represent the state of a dynamic system, for a physical quantity that can determine the range of values that can be taken, a state variable that does not deviate from the range may be determined. good. As a result, even when applied to the extended Kalman filter, it is possible to prevent the state vector from falling into a local optimum solution.

(3) Modification 3
In the present invention, the physical quantity representing the state of a dynamic system that estimates using a non-linear Kalman filter KF, quaternion q, geomagnetic strength r, geomagnetic dip θ representing the orientation, the angular velocity omega, the angular velocity sensor offset estimate g _o , and was offset estimate m _o of the geomagnetic sensor, the present invention is not limited to this and may be one that estimated for some of these.

(4) Modification 4
In the present invention, the observed value vector y _k is a vector whose elements are the observed value m that is the output of the geomagnetic sensor, the observed value a that is the output of the acceleration sensor, and the observed value g that is the output of the angular velocity sensor. The present invention is not limited to this, and the observation value vector may be generated using only a part of them.

DESCRIPTION OF SYMBOLS 1 ... Mobile phone, 70 ... Geomagnetic detection part, 80 ... Acceleration detection part, 90 ... Angular velocity detection part, KF ... Nonlinear Kalman filter, 100 ... Geomagnetic measurement program, 110 ... Initial value generation module, 120 ... Nonlinear Kalman filter module, 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 application unit.

Claims (4)

A plurality of sensors for observing the system and outputting observation values,
A state vector having a plurality of state variables indicating the state of the system as an element is estimated using a state transition model, an estimated observation value is calculated from the state vector using an observation value model, and the estimated observation value and the plurality of A Kalman filter that calculates a difference between the observed value of the sensor as an observation residual and executes an operation of updating the state vector based on the observation residual and the state vector,
A function that represents the strength of geomagnetism using the first variable and takes a value larger than the first value is defined as a first function.
Using the second variable to represent the dip angle of the geomagnetism, when the function that takes a value larger than the second value and smaller than the third value is the second function,
The first variable and the second variable are:
A geomagnetism measuring apparatus which is an element of the state vector. When the first variable is ρ and the second variable is η,
The first function is represented by exp (ρ),
The second function is represented by tan ⁻¹ η.
The geomagnetic measurement apparatus according to claim 1. A geomagnetism measurement method used in a geomagnetism measurement device including a plurality of sensors that observe a system and output observation values, respectively.
Using a state transition model to estimate a state vector whose elements are a plurality of state variables indicating the state of the system;
Calculate the estimated observation value from the state vector using the observation value model,
The difference between the estimated observation value and the observation value of the plurality of sensors is calculated as an observation residual,
Updating the state vector based on the observation residual and the state vector;
The first variable is used to represent the strength of the geomagnetism, and the function that takes a value larger than the first value is the first function, and the second variable is used to represent the dip of geomagnetism. Wherein the first variable and the second variable are elements of the state vector, when the function having a larger value and smaller than the third value is a second function. . A geomagnetism measurement program used in a geomagnetism measurement apparatus including a plurality of sensors that observe a system and output observation values, respectively.
Processing for estimating a state vector having a plurality of state variables indicating the state of the system using a state transition model;
A process of calculating an estimated observation value from the state vector using an observation value model;
A process of calculating a difference between the estimated observation value and the observation values of the plurality of sensors as an observation residual;
Processing to update the state vector based on the observation residual and the state vector;
The first variable is used to represent the strength of the geomagnetism, and the function that takes a value larger than the first value is the first function, and the second variable is used to represent the dip of geomagnetism. Wherein the first variable and the second variable are elements of the state vector, where the second function is a function that is larger and smaller than the third value. .

Images