JP2013122384A - Kalman filter and state estimation device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To perform accurate state estimation by using a Kalman filter in the case that temperature change occurs in a sensor.SOLUTION: A state estimation device included in a portable device 1 provided with a plurality of sensors including a three-dimensional geomagnetic sensor 70 with a first temperature detection unit 75 provided to estimate a state of a system includes a Kalman filter arithmetic unit 300. The Kalman filter arithmetic unit 300 includes a state transition model unit 410 for applying a state vector xto a state transition model to calculate an estimation state vector x, and an update unit 430 for calculating an updated state vector xon the basis of an observation residual error zcalculated on the basis of an observation value vector ywith output values of the plurality of sensors as factors and an estimation state vector x, and covariance Qof process noise wregulated in the state transition model unit 410 is determined on the basis of a temperature variation ΔTbeing a variation per unit time of temperature data Toutputted by the first temperature detection unit 75.

Description

  The present invention relates to a Kalman filter and a state estimation device.

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

  A method using a Kalman filter is known as a method for estimating the state of a dynamic system by integrating the outputs of a plurality of sensors that measure different physical quantities. For example, Patent Document 1 discloses a posture angle measurement device in which a triaxial angular velocity sensor, a triaxial acceleration sensor, and a nonlinear Kalman filter are mounted. In Non-Patent Document 1, output signals from a 3-axis angular velocity sensor, a 3-axis acceleration sensor, and a 3-axis geomagnetic sensor are integrated using an extended Kalman filter or a sigma point Kalman filter using unscented transformation, and the posture is integrated. An estimation method is disclosed.

  In general, the Kalman filter is a state transition model that estimates changes over time in multiple physical quantities that represent the state of a dynamic system, and multiple sensors in the dynamic system measure from the estimated state of the dynamic system. And an observation model for estimating a value to be performed (observed value). The Kalman filter is a value for estimating each of a plurality of physical quantities representing the state of the dynamic system using a difference (observation residual) between the estimated observation value and the observation value actually measured by the plurality of sensors. A state vector having (state variable) as an element is estimated and sequentially updated, so that the value indicated by each element of the state vector is approximated to an actual physical quantity (true value).

Japanese Patent Laid-Open No. 9-5104

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

Incidentally, a sensor such as a geomagnetic sensor or an angular velocity sensor may detect a component other than a component to be detected. In this case, in order to accurately determine the component that is the detection target of the sensor, a correction process that removes components other than the component that is the detection target from the output value of the sensor is necessary. The component removed in this correction process is called a sensor offset.
The sensor offset may change as the temperature of the sensor changes. When a sudden temperature change occurs in the sensor and the sensor offset changes abruptly, the value indicated by each element of the state vector deviates from the true value, and the Kalman filter can accurately estimate the state of the dynamic system. It may not be possible.

  The present invention has been made in view of the above points, and an object of the present invention is to provide a Kalman filter that can perform accurate state estimation when a temperature change occurs in a sensor.

  In order to solve the above-described problem, a Kalman filter according to the present invention is a three-dimensional magnetic sensor that detects magnetic components in three directions orthogonal to each other and sequentially outputs magnetic data expressed as vector data in a three-axis coordinate system. A Kalman filter used in a state estimation device that estimates a state of a system, and is incorporated in a device including a plurality of sensors and a temperature detection unit that detects temperatures of the three-dimensional magnetic sensor and sequentially outputs temperature data A state vector having a plurality of state variables including an offset estimation vector indicating an estimated value of the offset of the three-dimensional magnetic sensor is applied to a state transition model representing a change of the system over time. Thus, the state vector after the unit time has been estimated is calculated as an estimated state vector. And an observation residual based on an observation value vector having each of output values of the plurality of sensors as elements, and updating the state vector based on the estimated state vector and the observation residual An update unit that calculates this as a state vector after update, and when the amount of change in the temperature data in the unit time is a temperature change amount, the process noise defined by the state transition model Among the covariances, each component of the process noise covariance generated in the offset estimation vector is determined based on the temperature change amount.

According to the present invention, in the calculation of the Kalman filter using the estimated offset value of the three-dimensional magnetic sensor as an element (state variable) of the state vector, the covariance of the process noise defined by the state transition model is calculated using the temperature of the three-dimensional magnetic sensor. Determine based on the amount of change.
The offset of the three-dimensional magnetic sensor changes as the temperature of the three-dimensional magnetic sensor changes. Therefore, when a temperature change occurs in the three-dimensional magnetic sensor and the offset of the three-dimensional magnetic sensor changes, the value indicated by the state variable for estimating the offset of the three-dimensional magnetic sensor is true among the state vectors used in the calculation of the Kalman filter. Deviates from the value.
On the other hand, the Kalman filter according to the present invention determines the covariance of process noise defined in the state transition model based on the temperature change amount of the three-dimensional magnetic sensor 3 Since the state vector is updated in consideration of the offset change of the three-dimensional magnetic sensor, accurate state estimation can be performed.

  The Kalman filter according to the present invention detects a temperature of each of one or more sensors among the plurality of sensors and the plurality of sensors, and outputs temperature data corresponding to each of the one or more sensors. And a Kalman filter used in a state estimation device for estimating a system state, wherein a value obtained by estimating an offset of each of the one or more sensors among the plurality of sensors is used as an element. When a vector to be used is an offset estimation vector, a state vector having a plurality of state variables including the offset estimation vector as an element is applied to a state transition model representing a change of the system over time, so that unit time elapses. A state transition model unit that estimates the subsequent state vector and calculates this as an estimated state vector; and the plurality of sensors An observation residual is calculated based on an observed value vector having each force value as an element, the state vector is updated based on the estimated state vector and the observed residual, and this state vector is updated. And an update unit, and among the process noise covariances defined by the state transition model, each component of the process noise covariance generated in the offset estimation vector is one or more in the unit time The temperature data may be determined based on a temperature change amount of each of the temperature data.

According to this invention, the covariance of the process noise defined by the state transition model is one or more sensors in the calculation of the Kalman filter that updates the state vector including the estimated value of the offset of each of the one or more sensors. It is determined based on each temperature change amount.
In this case, since it is possible to update the state vector in consideration of the change in the offset of each of the one or more sensors accompanying the temperature change that occurs in each of the one or more sensors, accurate state estimation is possible. It becomes.

  In the above-described Kalman filter, the plurality of sensors include a three-dimensional magnetic sensor that detects magnetic components in three directions orthogonal to each other and sequentially outputs magnetic data represented as vector data in a three-axis coordinate system; A three-dimensional angular velocity sensor that detects angular velocities around three axes orthogonal to each other and sequentially outputs angular velocity data expressed as vector data in a three-axis coordinate system. It is preferable that at least one of a three-dimensional magnetic sensor or the three-dimensional angular velocity sensor is included.

  According to this invention, even if a temperature change occurs in the three-dimensional magnetic sensor or the three-dimensional angular velocity sensor in the calculation of the Kalman filter performed by the state estimation device incorporated in the device including the three-dimensional magnetic sensor and the three-dimensional angular velocity sensor, Accurate state estimation can be performed.

  Moreover, the state estimation apparatus according to the present invention includes any one of the Kalman filters described above, the plurality of sensors whose outputs are elements of the observation vector, and one or more sensors among the plurality of sensors. And a temperature detector that outputs one or more temperature data corresponding to the one or more sensors.

According to this invention, the covariance of the process noise defined by the state transition model is one or more sensors in the calculation of the Kalman filter that updates the state vector including the estimated value of the offset of each of the one or more sensors. It is determined based on each temperature change amount.
In this case, since the state estimation device can update the state vector in consideration of the change in the offset of each of the one or more sensors that occurs due to the temperature change that occurs in each of the one or more sensors, Accurate state estimation is possible.

  In the state estimation device described above, it is preferable that the temperature detection unit is disposed inside the one or more sensors among the plurality of sensors.

In the present invention, the temperature detection unit that detects the temperature of each of the one or more sensors may be disposed inside each of the one or more sensors, or may be disposed outside the one or more sensors. It may be a thing. That is, the temperature detection part should just detect the temperature of each of one or more sensors, and outputs the temperature data corresponding to each of one or more sensors per unit time.
Regardless of whether the temperature detection unit is arranged inside one or more sensors or arranged outside, the state estimation device is accompanied by a temperature change occurring in each of the one or more sensors. The state vector can be updated in consideration of the generated offset change of each of the one or more sensors, so that accurate state estimation is possible.

It is a block diagram which shows the structure of the portable apparatus which concerns on embodiment of this invention. It is a perspective view which shows the external appearance of the portable apparatus which concerns on embodiment of this invention. It is a functional block diagram showing the function of the Kalman filter calculating part which concerns on embodiment of this invention. It is a functional block diagram showing the function of the Kalman filter calculating part which concerns on the modification 1 of this invention. It is a block diagram which shows the structure of the portable apparatus which concerns on the modification 3 of this invention. It is a block diagram which shows the structure of the portable apparatus which concerns on the modification 4 of this invention.

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

[1. Device configuration and software configuration]
FIG. 1 is a block diagram of a portable device according to an embodiment of the present invention, and FIG. 2 is a perspective view showing an appearance of the portable device. The mobile device 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 orientation of the screen. This function is realized by calculating the Kalman filter based on the outputs of various sensors and estimating the orientation of the mobile device 1 and the direction of the geomagnetism Bg as viewed from the mobile device 1.

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

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

The three-dimensional magnetic sensor 70 includes an X-axis magnetic sensor 71, a Y-axis magnetic sensor 72, and a Z-axis magnetic sensor 73. Each sensor can be configured using an MI element (magnetoimpedance element), an MR element (magnetoresistance effect element), or the like. The magnetic sensor I / F 74 AD-converts output signals from the X-axis magnetic sensor 71, the Y-axis magnetic sensor 72, and the Z-axis magnetic sensor 73 and outputs magnetic data m. The magnetic data m is vector data represented by three components of the x axis, the y axis, and the z axis. More specifically, in the coordinate system fixed to the three-dimensional magnetic sensor 70 (hereinafter referred to as “sensor coordinate system Σ _S ”), the magnetic data m represents the output value from the X-axis magnetic sensor 71 as the x-axis component. The vector data represents the output value from the Y-axis magnetic sensor 72 as a y-axis component and the output value from the Z-axis magnetic sensor 73 as a z-axis component.
The three-dimensional magnetic sensor 70 includes a first temperature detection unit 75 that detects the temperature of the three-dimensional magnetic sensor 70. Magnetic sensor I / F 74 is an output signal from the first temperature detecting section 75 and AD conversion and outputs the temperature data _{T m.} That is, the first temperature detection unit 75 functions as a temperature detection unit that detects the temperature of the sensor included in the mobile device 1.

Incidentally, the magnetic data m detected by the three-dimensional magnetic sensor 70 includes geomagnetism Bg.
The geomagnetism Bg is a magnetic field having a horizontal component toward the magnetic pole north and a vertical component in the dip direction, and has a fixed orientation in a coordinate system fixed to a certain point on the ground (hereinafter referred to as “ground coordinate system Σ _G ”). and expressed as a vector ^G Bg having a size (the letter "G" subscript indicates the upper left of the sign of the vector means that the vector is a vector expressed in ground coordinate system sigma _G). That is, in the sensor coordinate system Σ _S , the geomagnetism Bg is expressed as a vector ^S Bg (μ) having a certain magnitude that changes its direction in conjunction with the attitude μ of the mobile device 1 (note that the upper left of the sign of the vector) letter "S" subscript indicates the means that the vector is a vector expressed in the sensor coordinate system sigma _S). Therefore, by calculating the vector ^S Bg (mu) representing the geomagnetism Bg in the sensor coordinate system sigma _S, it can be obtained of the portable device 1 posture mu.
On the other hand, the portable device 1 on which the three-dimensional magnetic sensor 70 is mounted is often provided with components that generate a magnetic field, such as various magnetized metals and electric circuits. Therefore, the vector data (magnetic data m) output from the three-dimensional magnetic sensor 70 is obtained by adding a vector representing the geomagnetism Bg and a vector representing the magnetic field (internal magnetic field Bi) generated by the component mounted on the portable device 1. It becomes a vector. Therefore, to calculate a vector ^S Bg (mu) representing the geomagnetism Bg in the sensor coordinate system sigma _S, in order to obtain the geomagnetism Bg direction as viewed from the portable device 1 correctly, the sensor coordinate system sigma _S, 3-dimensional magnetic A correction process for subtracting the vector ^S Bi representing the internal magnetic field Bi generated by the component of the mobile device 1 from the coordinates indicated by the magnetic data m output from the sensor is required.
Thus, in order to specify the correct orientation of the geomagnetism Bg that is the detection target, the vector ^S Bi representing the internal magnetic field Bi removed from the magnetic data m output from the three-dimensional magnetic sensor 70 in the correction process is represented by 3 This is called an offset of the dimensional magnetic sensor 70.

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

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

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

FIG. 3 is a functional block diagram showing functions realized by the CPU 10 executing the state estimation program 100 in the state estimation apparatus. As illustrated in FIG. 3, the state estimation device includes an initial value generation unit 200, a Kalman filter calculation unit 300, and an output information generation unit 500.
The initial value generation unit 200 generates an initial vector INI based on various setting information and outputs it.
The Kalman filter calculation unit 300 uses magnetic data m output from the three-dimensional magnetic sensor 70, acceleration data a output from the three-dimensional acceleration sensor 80, and angular velocity data g output from the three-dimensional angular velocity sensor 90 as elements. Using the value vector y, the state of the system is estimated by executing an operation of a nonlinear Kalman filter that sequentially updates the state vector x having the initial vector INI as an initial value.
The output information generation unit 500 calculates output information such as the direction of the geomagnetism Bg based on the state vector x periodically output from the Kalman filter calculation unit 300.

Here, the state vector x is a vector having a plurality of state variables as elements. Each of the plurality of state variables that are elements of the state vector x represents a plurality of physical quantities indicating the state of the system estimated by the nonlinear Kalman filter. That is, the state vector x is a vector representing the state of the system estimated by the nonlinear Kalman filter.
In this embodiment, as the elements (state variables) of the state vector x, the attitude μ of the mobile device 1, the geomagnetic strength r, the geomagnetic dip angle φ, the angular velocity ω of the mobile device 1, and the offset estimation value of the three-dimensional angular velocity sensor 90. g _OFF and the estimated offset value m _OFF of the three-dimensional magnetic sensor 70 are adopted, and these are used as estimation targets.
The state vector x _{k at} time T = k is expressed by the following equation (1). The subscript “k” attached to the lower right of various variables, vectors, matrices, and the like means that the values of the various variables, vectors, matrices, etc. at time T = k.

Here, the geomagnetism strength r is a scalar, the geomagnetic dip angle φ is a scalar, the angular velocity ω is a three-dimensional vector, the offset estimated value g _OFF of the three-dimensional angular velocity sensor 90 is a three-dimensional vector, 3 The estimated offset value m _OFF of the three-dimensional magnetic sensor 70 is a three-dimensional vector.
In addition, the posture μ is expressed using quaternions. A quaternion is a four-dimensional number that represents the posture (rotation state) of an object. For example, the respective axes of the sensor coordinate system sigma _S, the attitude mu where the respective axes of the ground coordinate system sigma _G match is defined as a reference position, the reference orientation mu = expressed as (0, 0, 0, 1) . At this time, the arbitrary posture μ of the mobile device 1 can be expressed as a posture rotated by an angle ψ with the direction of the unit vector ρ as the rotation axis with respect to the reference posture. The posture μ after rotation is given by Equation (2) using a quaternion.

The observed value vector y includes magnetic data m output from the three-dimensional magnetic sensor 70, acceleration data a output from the three-dimensional acceleration sensor 80, and angular velocity data g output from the three-dimensional angular velocity sensor 90 as elements. It is a vector to do. Observation value vector y _{k at} time T = k is given by equation (3).

The Kalman filter operation unit 300 performs the operation of the nonlinear Kalman filter using the state vector x _k and the observed value vector y _k described above, and obtains the state vector x _{k so} that each element of the state vector x _k approaches a true value. By periodically updating, a plurality of physical quantities representing the system state are estimated.
Here, the “true value” is a value representing an actual physical quantity corresponding to each element of the state vector x _k . Hereinafter, a vector in which each element of the state vector x _k is a true value is referred to as a “true value of the state vector”. Details of the operation of the nonlinear Kalman filter will be described later.

The initial value generation unit 200 generates an initial vector INI and outputs it.
The initial vector INI is the initial value of the state vector x _k , that is, the state vector x ₀ at time T = 0, the initial value μ ₀ of the posture μ, and the geomagnetism strength r, as shown in the following equation (4). the initial value _{r 0} of the initial value phi ₀ geomagnetic dip phi, the angular velocity omega initial value omega _0, the initial value of the offset estimation value _{g OFF} of the three-dimensional angular velocity sensor 90 _{g OFF, 0,} and the three-dimensional magnetic sensor 70 The initial value m _{OFF, 0} of the offset estimated value m _OFF is included as an element.

  It should be noted that each element of the initial vector INI may be determined as appropriate so as not to greatly deviate from the true value. In the following, a setting example of each element of the initial vector INI is shown.

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

The initial value ω ₀ of the angular velocity ω is set to “0”, for example, assuming that the mobile device 1 is stationary.
Further, the initial value g _{OFF, 0} of the offset estimated value g _OFF of the three-dimensional angular velocity sensor 90 is normally adjusted in the vicinity of “0”, so is set to “0”.
As the initial value μ ₀ of the posture μ, for example, assuming that the mobile device 1 is stationary in a certain direction, a value that reduces the deviation from the actual initial posture is set.

Initial value _{m OFF,} the offset estimate _{m OFF} of the three-dimensional magnetic sensor 70 _0, magnetic data _{m 0} is output at time T = 0 from a 3D magnetic sensor 70, representing the geomagnetism Bg in ground coordinate system sigma _G Using the vector ^G Bg and the matrix B (μ), it is calculated by the following equation (5).
Here, the matrix B (mu), when the portable device 1 is attitude mu, a vector expressed in ground coordinate system sigma _G, a coordinate transformation matrix for representing the sensor coordinate system sigma _S, below (6) A vector ^G Bg representing the geomagnetism Bg at the time T = 0 from the ground coordinate system Σ _{G is} expressed as follows using an initial value r ₀ of the geomagnetic strength r and an initial value φ ₀ of the geomagnetic dip angle φ: (7) In the present embodiment, the ground coordinate system sigma _G, oriented magnetic poles north y-axis, z-axis defined as facing upward vertical.

In this way, the initial value generation unit 200 generates the initial vector INI and outputs it. Then, the Kalman filter calculation unit 300 estimates the state of the system by executing a nonlinear Kalman filter calculation that updates the state vector x _k with the initial vector INI as an initial value. Hereinafter, details of the calculation of the nonlinear Kalman filter performed by the Kalman filter calculation unit 300 will be described.

[2. Calculation using nonlinear Kalman filter]
In general, the Kalman filter uses a state transition model that represents a change in the state of a dynamic system over time, and a unit from a state vector x ⁺ _k−1 that indicates the state of the system at a certain time (time T = k−1). The state vector x _k after the elapse of time (time T = k) is estimated. This estimated value is referred to as an estimated state vector x ⁻ _k .
Then, the Kalman filter estimates the observation value vector y _k from the estimated state vector x ⁻ _k using an observation model that represents the relationship between the observation value vector y _k and the state vector x _k whose elements are outputs from various sensors. . This estimated value is referred to as an estimated observation value vector y ⁻ _k .
Next, the Kalman filter estimated observed value vector y ^- and _k, calculates the difference between the observed value vector y _k to the actual observed value element as observed residuals z _k, based on the observation residuals z _k Kalman The gain _Kk is calculated. Furthermore, the Kalman filter updates the state vector x ⁺ _k after updating the state vector x ⁺ _k−1 using the observation residual z _k , the Kalman gain K _k , and the estimated state vector x ⁻ _k. calculate.
By the Kalman filter operation that repeatedly updates the state vector x _k as described above, each element of the state vector x _k is brought close to an accurate value close to a value (true value) that accurately represents the actual physical quantity.

Although details will be described later, in this embodiment, a sigma point Kalman filter which is a nonlinear Kalman filter is used as the Kalman filter.
The sigma point Kalman filter first sets a plurality of sigma points χ ⁺ _k−1 around the state vector x ⁺ _k−1 and applies the plurality of sigma points χ ⁺ _k−1 to the state transition model. Estimate multiple sigma points after unit time. The plurality of estimated sigma points are referred to as estimated sigma points χ ⁻ _k . Next, the sigma point Kalman filter obtains an estimated state vector x ^- _k by calculating an average of a plurality of estimated sigma points χ ^- _k .
Therefore, when the Kalman filter operation is performed using the sigma point Kalman filter, the estimated observation value vector y ⁻ _k is strictly based on a plurality of estimated sigma points χ ⁻ _k existing around the estimated state vector x ⁻ _k. Is calculated.

In this embodiment, the state transition model of the nonlinear Kalman filter is given by the following equation (8), and the observation model is given by the following equation (9). In the present embodiment, the state equation f appearing in Equation (8) and the observation equation h appearing in Equation (9) are non-linear functions.

Here, the state vector x _k is a dim (x) -dimensional vector, and the observation value vector y _k is a dim (y) -dimensional vector. In the present embodiment, dim (x) = 15 and dim (y) = 9.
Here, P ⁻ _k is the covariance of the estimated error of the estimated state vector x ⁻ _k , and P ⁺ _k is the covariance of the estimated error of the updated state vector x ⁺ _k . P ^yy _k is a covariance of the observation residual z _k , and P ^xy _k is a mutual covariance matrix of the estimated state vector x ⁻ _k and the estimated observation value vector y ⁻ _k .
Further, the process noise w _k appearing in the equation (8) and the observation noise v _k appearing in the equation (9) are Gaussian noises centered on “0”.
Equation (8) shows that the state vector x _k at time T = k is obtained by substituting the state vector x _k-1 at time T = _k−1 for the state equation f, and the time T = k−1. It is estimated by adding the process noise w _{k−1 in} FIG.
Equation (9) shows that the observed value vector y _k at time T = k is obtained by substituting the state vector x _k at time T = _k into the observation equation h, and the observed noise v at time T = k. It shows that it is estimated by adding _k .
Incidentally, representing the covariance of the process noise _{w k Q k,} the covariance of measurement noise _{v k} and _{R k.}

The covariance Q _k of the process noise w _k is expressed by 15 rows and 15 columns composed of a matrix Q _{cst, 1} , a matrix Q _{cst, 2} , and a matrix Q _{m, k} as shown in the following equation (10). It is a symmetric matrix.
Here, the matrix Q _{cst, 1} is a 12 _× 12 symmetric matrix, and each component of the matrix Q _{cst, 1} is a constant. This matrix Q _{cst, 1} represents the covariance of process noise generated in elements other than the estimated offset value m _{OFF, k} of the three-dimensional magnetic sensor 70 among the process noise w _k generated in the state vector x _k in the state transition model. It is a matrix. The matrix Q _{cst, 2} is a 12 _× 3 zero matrix.
The matrix Q _{m, k} is a symmetric matrix with 3 rows and 3 columns, and the process that occurs in the offset estimation value m _{OFF, k} of the three-dimensional magnetic sensor 70 among the process noise w _k that occurs in the state vector x _k in the state transition model. It is a matrix representing the covariance of noise. The matrix Q _{m, k} is determined based on the temperature change ΔT _{m, k} of the three-dimensional magnetic sensor 70 as shown in the following equation (11). The coefficient α _m appearing in the equation (11) is a positive real number. Further, the matrix Q _{cst, m} represents the process noise w _k generated in the state vector x _k in the state transition model that assumes that the offset of the three-dimensional magnetic sensor 70 does not change even if the temperature of the three-dimensional magnetic sensor 70 changes. Of these, the matrix represents the covariance of the process noise generated in the offset estimation value m _{OFF, k} of the three-dimensional magnetic sensor 70. In the present embodiment, each component of the matrix Q _{cst, m} is a constant.
Further, as shown in the equation (12), the temperature change amount ΔT _{m, k} is the temperature data T _{m, k} at time T = k and the temperature data T _{m, k−1 at} time T = k−1. Represents the absolute value of the difference. Note that when subjecting the letter "k" subscripts to temperature variation [Delta] T _m occurs between time T = k and time T = k-1, the absolute value of the temperature change amount of the three-dimensional magnetic sensor 70 It represents that.

The offset of the three-dimensional magnetic sensor 70 changes as the temperature of the three-dimensional magnetic sensor 70 changes. More specifically, the magnitude of the offset change of the three-dimensional magnetic sensor 70 is determined based on the magnitude of the temperature change amount ΔT _{m, k} of the three-dimensional magnetic sensor 70.
Although details will be described later, it is difficult to formulate a change with time of the offset of the three-dimensional magnetic sensor 70. That is, it is difficult to formulate as a state equation f after taking into account the change over time of the offset of the three-dimensional magnetic sensor 70 up to the temperature change of the three-dimensional magnetic sensor 70.
When the state equation f does not take into account the temperature change of the three-dimensional magnetic sensor 70, the state transition model is a change with time of the system estimated by the state estimation device when a temperature change occurs in the three-dimensional magnetic sensor 70. Cannot be expressed accurately. If the state transition model does not accurately represent changes over time in the system, the state vector x _k is updated by trusting the state transition model, and each element of the state vector x _k (particularly, the three-dimensional magnetic field). The estimated offset value m _{OFF, k} ) of the sensor 70 is likely to be a value deviating from the true value.
Thus, if each element of the state vector x _k is a value which deviates from the true value, the state estimation device may not accurately estimate the state of the system. Further, when each element of the state vector x _k becomes a value deviating from the true value, even if the calculation of the nonlinear Kalman filter is repeated thereafter, until each element of the state vector x _k converges to a value close to the true value. It will take a long time.

On the other hand, in the present embodiment, as shown in Expression (11), the matrix Q that is a component corresponding to the offset estimated value m _OFF of the three-dimensional magnetic sensor 70 in the covariance Q _k of the process noise w _{k. m,} and _k, determined based on the temperature change amount [Delta] T _{m, k} of the three-dimensional magnetic sensor 70.
In this case, the state transition model considers the change in the offset of the three-dimensional magnetic sensor 70 accompanying the temperature change of the three-dimensional magnetic sensor 70 that cannot be formulated in the state equation f as the process noise w _k . Thereby, the state estimation device according to the present embodiment updates the state vector x _{k in} consideration of an error existing between the state transition model and a system change with time estimated by the state estimation device. Therefore, the possibility that each element of the updated state vector x ⁺ _k deviates from the true value can be reduced.

Of the state variables representing values for estimating the sensor offset, a state variable in which the covariance of process noise occurring in the state variable is determined based on the temperature change amount of the sensor is referred to as an “offset estimation vector”. There is a case.
In the present embodiment, the state variables representing the values for estimating the sensor offset are the offset estimated value g _OFF of the three-dimensional angular velocity sensor 90 and the offset estimated value m _OFF of the three-dimensional magnetic sensor 70, whereas the sensor The state variable in which the covariance of process noise generated in the state variable based on the temperature change amount is determined as the offset estimated value m _OFF of the three-dimensional magnetic sensor 70. That is, in this embodiment, the offset estimated value m _OFF of the three-dimensional magnetic sensor 70 corresponds to the offset estimation vector.

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

Note that the estimated observation value vector y ⁻ _k is a value calculated by applying the estimated state vector x ⁻ _k (strictly, the estimated sigma point χ ⁻ _k ) to the observation model shown in Expression (9). . Therefore, the observation residual z _{k that} is the difference between the estimated observation vector y ⁻ _k and the observation vector y _k based on the actual sensor output is a value that accurately represents the estimated state vector x ⁻ _k and the actual physical quantity ( This is a value indicating the degree of approximation to the true value.
Therefore, as shown in Expression (15), by using the observation residual z _k , the estimated state vector x ⁻ _k is updated to the updated state vector x ⁺ _k , whereby each element of the state vector x _k Can be brought closer to the true value.

The Kalman filter operation unit 300 performs an operation of a nonlinear Kalman filter using a sigma point Kalman filter using unscented transformation.
As illustrated in FIG. 3, the delay unit 310 delays the updated state vector x ⁺ _k output from the adder 390 by a unit time (time corresponding to time T = k from time T = k−1). As a result, a state vector x ⁺ _k−1 is generated and output to the sigma point generation unit 320. In the first calculation (calculation at time T = 0), the delay unit 310 applies the initial vector INI output from the initial value generation unit 200 to the state vector x ⁺ _k−1 .

The sigma point generation unit 320 applies the temperature data T _{m, k} sequentially output by the three-dimensional magnetic sensor 70 to the equation (11), and calculates the matrix Q _{m, k−1} to obtain the equation (10). The covariance Q _k−1 of the process noise w _k is calculated.

In addition, the sigma point generation unit 320 uses the covariance matrices P ⁺ _k−1 and Q _k−1 in “dim (x)” rows and “dim (x)” columns to generate “2 dim (x) +1” pieces. Generate sigma points.
Specifically, first, with respect to the average of a plurality of sigma points, using the scaling parameter λ representing the spread of the sigma points around the average, the vector σ _k (1) expressed by the equations (17) and (18) is used. ) To σ _k (2dim (x)).

At this time, the sigma point generation unit 320 performs “2dim (x) +1” sigma expressed by Expression (19) and Expression (20) based on the vector σ _k−1 and the state vector x ⁺ _k−1. Points χ ⁺ _k−1 (0) to χ ⁺ _k−1 (2dim (x)) are generated.

State transition operation unit 330, as shown in equation (21), "2dim (x) +1" number of sigma points χ ⁺ _k-1 (0) at time _{^{T = k-1 ~χ + k}} -1 (2dim (x) each), by applying the state equation f, time T = "2Dim (x) +1" in the k estimated sigma points _{^{χ - k (0) ~χ -}} k (2dim (x)) Is calculated.

Then, the estimated state vector calculating unit 340, as shown in equation (22), time T = "2dim (x) +1" in the k estimated sigma points _{^{χ - k (0) ~χ -}} k (2dim ( The estimated state vector x ^- _k is calculated by calculating the average value of x)).

Further, the estimated state vector calculating unit 340, as shown in equation (23), the estimated state vector x ^- calculates the _k ^- covariance P of the estimation error of _k. Here, the vector xi] _j, the following is shown in equation (24), the estimated sigma point chi ^- a vector representing the difference between the _k ^- _{k (j)} and the estimated state vector x.

As described above, the sigma point generation unit 320, the state transition calculation unit 330, and the estimated state vector calculation unit 340 have the covariance Q _{k−1 of the} process noise w _k generated in the state vector x ⁺ _k−1 in the state transition model. to calculate the estimated state vector x by applying the state vector x ^{+ _k-1} to the state transition model ^- calculating a _k, which functions as a state transition model 410.

On the other hand, the estimated observation value calculating unit 350, as shown in equation (25), time T = "2dim (x) +1" in the k estimated sigma points _{^{χ - k (0) ~χ -}} k (2dim (x each)), by applying to the observation equation h, to calculate the "2dim (x) +1" number of estimated observations _{γ k (0) ~γ k (} 2dim (x)).

Then, the estimated observed value vector calculation unit 360, as shown in equation (26), the average of "2dim (x) +1" number of estimated observations _{γ k (0) ~γ k (} 2dim (x)) calculation By doing so, the estimated observation value vector y ^- _k is calculated.

In addition, the estimated observation value vector calculation unit 360 calculates the covariance P ^yy _k of the observation residual as shown in Expression (27). Here, the vector ζ _j is a vector representing the difference between the estimated observation value γ _k (j) and the estimated observation value vector y ⁻ _k , as shown in the following equation (28).

Thus, the estimated observation value calculating unit 350, and the estimated observed value vector calculating unit 360 estimates the sigma points _{^{χ - k (0) ~χ -}} k be applied to each observation model (2dim (x)) Functions as an observation model unit 420 that calculates the estimated observation value vector y ⁻ _k .

The subtractor 370 calculates an observation residual z _k as a difference between the observed value vector y _k and the estimated observed value vector y ⁻ _k , as shown in Expression (13).
The Kalman gain assigning unit 380 calculates the mutual covariance matrix P ^xy _k as shown in Expression (29). Then, as shown in Expression (14), the Kalman gain assigning unit 380 calculates the Kalman gain K _k based on the covariance P ^yy _k of the observation residual and the mutual covariance matrix P ^xy _k , The calculation of the second term (K _k z _k ) on the right side of Expression (15) is executed. Moreover, Kalman gain applying unit 380, as shown in equation (16), the covariance _{P k} of the estimation error of the state vector _{x k,} P ^- updates from _k to ^P _{+ k.}

The adder 390 adds the estimated state vector x ⁻ _k and the second term (K _k z _k ) on the right side of the equation (15) output from the Kalman gain assigning unit 380, as shown in the equation (15). Thus, the updated state vector x ⁺ _k is calculated.

As described above, the subtractor 370, the Kalman gain assigning unit 380, and the adder 390 use the observation residual z _k , the Kalman gain K _k , and the estimated state vector x ⁻ _k to _generate a state vector x ⁺ _{k−. It} functions as an updating unit 430 that calculates the updated state vector x ⁺ _k that updates ₁ .

  Next, of the state transition models used in the state transition model unit 410, the state equation f used in the calculation in the state transition calculation unit 330 will be described.

First, of the state equation f, from the posture ^mu _{+ k-1} at time T = k-1, the posture mu at time T = k after the lapse of the unit time ^- calculation for estimating the _k is given by the following equation (30) Represented as: Here, mu ^- _k is estimated state vector x at time k ^- of _k, an element corresponding to the state variable representing the attitude mu.
Note that the operator Ω on the right side of the equation (30) is defined by the equation (31). Here, I _{3 × 3} represents a unit matrix of 3 rows and 3 columns. For the three-dimensional vector l = (l ₁ , l ₂ , l ₃ ), the operator [l ×] is defined by equation (32). Further, unit time (measurement time interval from time T = k−1 to time T = k) is represented by Δt, and corresponds to a state variable representing an angular velocity in the state vector x ⁺ _k after the update at time T = k. When the element is represented by ω ⁺ _k , the component Ψ ⁺ _k constituting the operator Ω is defined by Expression (33).

The attitude μ is expressed by a quaternion and needs to satisfy the normalization condition || μ || = 1 as shown in the equation (2). However, when the state vector x _k is updated using the sigma point Kalman filter, the updated state vector x ⁺ _k is calculated as an average of the estimated sigma points χ ⁻ _k (j) as shown in Equation (22). that the estimated state vector x ^- since that is determined based on _k, the norm of the (pre-update) the state vector x ^{+ _k-1,} and the norm of the state vector x ⁺ _k after update, it may become different values . Accordingly, when the sigma point Kalman filter is calculated, the norm of the posture μ ⁺ _k−1 that is an element of the state vector x ⁺ _k−1 (before update) and the posture that is an element of the state vector x ⁺ _k after update. There is a possibility of a different value from the norm of μ ⁺ _k . That is, when performing the sigma point Kalman filter calculation, there is a possibility that the normalization condition for the posture μ is not satisfied.
Therefore, when any calculation is performed on the posture μ, the result after the calculation is normalized by the size of the vector itself.
In order to maintain the normalization condition more strictly, the posture μ _k among elements constituting the state vector x _k is limited to only the difference information from the previous time using MRPs (modified Rodrigues parameters), and the Kalman filter May be updated based on the difference information obtained from the Kalman filter.

It is difficult to predict changes in the geomagnetic strength r and the geomagnetic dip angle φ. Therefore, in the state transition model according to the present embodiment, for the sake of convenience, the geomagnetic strength r _k and the dip angle φ _k at time T = k, and the geomagnetic strength r _k-1 and the dip angle φ _{k at} time T = k−1. It is assumed that ₋₁ is an equal value.
Similarly, it is difficult to predict a change in the estimated offset value g _OFF of the three-dimensional angular velocity sensor 90. Therefore, in the state transition model according to the present embodiment, for convenience, the estimated offset value g _{OFF, K} of the three-dimensional angular velocity sensor 90 at time T = k and the estimated offset value of the three-dimensional angular velocity sensor 90 at time T = k−1. Assume that g _{OFF, K−1} is equal.

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

As described above, the offset of the three-dimensional magnetic sensor 70, the direction and magnitude of the internal magnetic field Bi to the portable device 1 of the component emits a vector expressed in the sensor coordinate system sigma _S. Therefore, the offset of the three-dimensional magnetic sensor 70 does not change while the internal state of the mobile device 1 is constant. On the other hand, when the internal state of the mobile device 1 changes, for example, when the magnitude of the current flowing through the component mounted on the mobile device 1 changes, or the magnetization status of the component mounted on the mobile device 1 changes. In this case, the offset of the three-dimensional magnetic sensor 70 also changes. That is, the internal state of the mobile device 1 changes depending on the operation of the mobile device 1 by the user of the mobile device 1, the environment outside the mobile device 1, and the like. Therefore, it is difficult to predict the timing at which the offset of the three-dimensional magnetic sensor 70 changes and the amount of offset change of the three-dimensional magnetic sensor 70, and the offset estimated value m of the three-dimensional magnetic sensor 70 at time T = k−1. It is difficult to formulate the offset estimated value m _{OFF, K} of the three-dimensional magnetic sensor 70 at time T = k using _{OFF, K−1} .
Therefore, in the state transition model according to the present embodiment, for convenience, the estimated offset value m _{OFF, K} of the three-dimensional magnetic sensor 70 at time T = k and the estimated offset value of the three-dimensional magnetic sensor 70 at time T = k−1. Assume that m _{OFF, K−1} is equal.

As described above, the state equation f in the state transition model according to the present embodiment is a state variable representing the posture μ _k among a plurality of state variables constituting the state vector x _k as shown in the following equation (34). Other than the above, it is formulated as an equation indicating that it does not change from the previous time.

As described above, when the state vector x _k is applied to the state equation f, the value indicated by each element other than the posture μ _{k in} the state vector x _k does not change.
However, each element of the state vector x _k other than the posture μ _k is updated by the adder 390 so as to approach the true value based on the observation residual z _k . That is, as shown in Expression (15), each element of the state vector x _k includes an estimated state vector x ⁻ _k calculated by the state transition model and output values (observed value vectors y _k ) from a plurality of sensors. Based on both of the observation residuals z _k calculated using the values, the values are updated so as to approach the true value.

As shown in equation (15), the estimated state vector x ^- of _k and observation residuals _{z k,} the rate of contribution to the state vector ^x _{+ k} after update is determined by the Kalman gain _{K k.}
In other words, the Kalman gain K _k if it is possible to properly determine the components of the estimated state vector x accounts for the state vector x ⁺ _k after update ^- _k and the ratio of the observed residuals occupying the state vector x ⁺ _k after update The ratio of z _k is appropriately adjusted, and it is possible to prevent each element of the updated state vector x ⁺ _k from deviating from the true value.

By the way, when the state equation used in the state transition model cannot accurately represent the change over time of the system estimated by the state estimation device, each element of the estimated state vector x ⁻ _k calculated using the state equation Is likely to be a value deviating from the true value.
In this case, for example, each component of the Kalman gain K _k is set to a large value, and each element of the second term (K _k z _k ) in Equation (15) is set to a large value, so that the updated state vector x ⁺ _k The ratio of the estimated state vector x ^- _k to the relative value is relatively small. Thereby, it becomes possible to prevent each element of the updated state vector x ⁺ _k from deviating from the true value.

The state equation f according to the present embodiment is an equation representing that the estimated offset value m _OFF of the three-dimensional magnetic sensor 70 is a constant value (not changing with time) as shown in the equation (34). It is. However, actually, when a temperature change occurs in the three-dimensional magnetic sensor 70, the offset (true value) of the three-dimensional magnetic sensor 70 changes. The magnitude of the offset change of the three-dimensional magnetic sensor 70 is determined based on the magnitude of the temperature change amount ΔT _{m, k} of the three-dimensional magnetic sensor 70. That is, when the temperature change amount ΔT _{m, k} is a large value, the estimated offset value m _OFF of the three-dimensional magnetic sensor 70 calculated using the state equation f is the actual value of the offset of the three-dimensional magnetic sensor 70 ( (True value) is calculated as a deviated value. That is, when the temperature change amount ΔT _{m, k} is a large value, the state equation f cannot accurately represent the change over time in the physical quantity representing the system estimated by the state estimation device.

Therefore, in the present embodiment, as shown in Expression (10), Expression (11), and Expression (12), the process noise w _k is shared based on the temperature change amount ΔT _{m, k} of the three-dimensional magnetic sensor 70. A variance Q _k is determined.
The process noise w _k expresses an error that exists between the state equation f and the change over time of the physical quantity representing the system estimated by the state estimation device. Therefore, the error existing between the state equation f and the change with time of the system due to the temperature change of the three-dimensional magnetic sensor 70 is expressed by the process noise w _k (and its covariance Q _k ). It becomes possible.
Although details will be described later, the Kalman gain K _k is determined based on the covariance Q _k−1 of the process noise w _k−1 . Therefore, the Kalman gain K _k is determined based on the temperature change amount ΔT _{m, k−1} . As a result, in the update of the state vector x _k using the Kalman gain K _k shown in equation (15), the updated state vector x ⁺ _k is derived from the state equation f due to the temperature change of the three-dimensional magnetic sensor 70. It is calculated in consideration of an error that occurs between changes in the system over time.
That is, according to the present embodiment, even if an error occurs between the state equation f and the change over time of the system due to the temperature change of the three-dimensional magnetic sensor 70, the error is taken into consideration. Thus, since the updated state vector x ⁺ _k is determined, the possibility that each element of the updated state vector x ⁺ _k deviates from the true value can be reduced.

Hereinafter, it will be described that the Kalman gain K _k is determined based on the covariance Q _k−1 of the process noise w _k−1 .
First, as shown in equations (17) and (18), the magnitude of the spread with respect to the average of a plurality of sigma points χ ⁺ _k−1 (1) to χ ⁺ _k−1 (2dim (x)) It is determined based on the covariance Q _k−1 of the noise w _k−1 . Then, as shown in Expression (21), Expression (22), and Expression (24), the spread of a plurality of sigma points χ ⁺ _k−1 (1) to χ ⁺ _k−1 (2dim (x)) is large. Based on this, each of the vectors ξ _{1 to} ξ _{2 dim (x)} is determined. Further, as shown in equation (23), the estimated state vector x ^- _k of the estimation error covariance P ^- _k is determined based on the vector _{ξ 1} ~ξ 2dim _(x). Thus, the estimated state vector x ^- covariance P of _k of the estimation error ^- _k is determined based on the covariance _{Q k-1} of the process noise _{w k-1.}
Similarly, as shown in Expression (25), Expression (26), and Expression (28), the spread of a plurality of sigma points χ ⁺ _k−1 (1) to χ ⁺ _k−1 (2dim (x)) Based on the magnitude, each of vectors ζ _{1 to} ζ _{2 dim (x)} is determined. As shown in Expression (29), the mutual covariance matrix P ^xy _k is determined based on the vectors ξ _{1 to} ξ _{2 dim (x)} and the vectors ζ _{1 to} ζ _{2 dim (x)} . Further, as shown in Expression (27), the covariance P ^yy _k of the observation residual is determined based on the vectors ζ _{1 to} ζ _{2dim (x)} . Thus, the mutual covariance matrix P ^xy _k and the observation residual covariance P ^yy _k are determined based on the covariance Q _k−1 of the process noise w _k−1 .
As shown in the equation (14), the Kalman gain K _k is determined based on the mutual covariance matrix P ^xy _k and the observation residual covariance P ^yy _k . Therefore, the Kalman gain K _k is determined based on the covariance Q _k−1 of the process noise w _k−1 .

If the covariance Q _{k−1 of} the process noise w _k−1 is determined without considering the temperature change ΔT _{m, k−1} , each component of the Kalman gain K _k is also the temperature change ΔT _{m, It} is determined without considering _k-1 . In this case, since the updated state vector x ⁺ _k is also determined without considering the temperature variation ΔT _{m, k−1} , the values of the updated state vector x ⁺ _k deviate from the true values. Is likely.
On the other hand, as described above, the state estimation device according to the present embodiment determines the covariance Q _k−1 of the process noise w _k−1 based on the temperature change amount ΔT _{m, k−1} .
Specifically, among the covariance Q _k−1 of the process noise w _k−1 generated in the state vector x _k in the state transition model, the process noise generated in the offset estimated value m _{OFF, k} of the three-dimensional magnetic sensor 70 is shared. The matrix Q _{m, k−1} representing the variance is determined based on the temperature change amount ΔT _{m, k−1} . Accordingly, each value of the component corresponding to the estimated offset value m _{OFF, k} of the three-dimensional magnetic sensor 70 in the Kalman gain K _k is determined based on the temperature change amount ΔT _{m, k−1} .
As a result, the element corresponding to the offset estimated value m _OFF ⁺ _k of the three-dimensional magnetic sensor 70 in the updated state vector x ⁺ _k takes into account the temperature change ΔT _{m, k−1} of the three-dimensional magnetic sensor 70. _Therefore , the element corresponding to the offset estimated value m _OFF ⁺ _k of the three-dimensional magnetic sensor 70 in the updated state vector x ⁺ _k is the actual value (true value) of the offset of the three-dimensional magnetic sensor 70. Can be prevented from being calculated as a deviating value.

  Next, among the observation models used in the observation model unit 420, the observation equation h used in the calculation in the estimated observation value calculation unit 350 will be described.

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

Furthermore, the vector ^G Bg of the ground coordinate system sigma _G represents the geomagnetism Bg is given by equation (36). Therefore, the estimated value γ _mag of the magnetic data m output from the three-dimensional magnetic sensor 70 is obtained by using the offset estimated value m _OFF of the magnetic sensor and the matrix B (μ) expressed by the expression (6). 37).

Further, in the observation model, the estimated value γ _acc of the acceleration data a output from the three-dimensional acceleration sensor 80 is expressed by the equation (6) using the matrix B (μ) expressed by the equation (6) and the vector ^G G _RV. 38). The vector ^G G _RV is a three-dimensional vector obtained by normalizing the vector representing the gravitational acceleration in the ground coordinate system Σ _{G with} the magnitude of the gravitational acceleration, as shown in the following equation (39).

Thus, the observation equation h in the observation model according to the present embodiment is formulated by the equations (35), (37), and (38).
Then, substituting Equation (3) into the first term on the right side of Equation (13) and expressing the second term on the right side of Equation (13) using Equation (35), Equation (37), and Equation (38). Thus, the equation (13) can be transformed into the following equation (40). At this time, the observation residual z _k is calculated by Expression (40).

[3. About output information]
The output information generation unit 500 calculates the attitude μ of the mobile device 1 based on the state vector x _k periodically output by the Kalman filter calculation unit 300, and outputs this as output information.
In the present embodiment, the output information generation unit 500 outputs the posture μ as output information. However, the present invention is not limited to such a form, and output values and states of various sensors included in the mobile device 1. Information calculated based on the vector x _k may be output information.
For example, the output information generation unit 500 may calculate the direction of geomagnetism Bg viewed from the mobile device 1 as output information. Specifically, the output information generation unit 500 _{converts the} magnetic data m _k output from the three-dimensional magnetic sensor 70 and the estimated offset value m _{OFF, k} of the three-dimensional magnetic sensor 70 that is an element of the state vector x _k. based on, then calculates a vector ^S Bg representing the geomagnetic Bg in the sensor coordinate system sigma _S, it may be configured to output as output information.

[4. Conclusion]
As described above, the state estimation device according to the present embodiment sets the value of each component of the matrix Q _{m, k} based on the temperature change amount ΔT _{m, k} of the three-dimensional magnetic sensor 70.
When the temperature change amount ΔT _{m, k} of the three-dimensional magnetic sensor 70 becomes a large value, a change with time of the estimated offset value m _OFF of the three-dimensional magnetic sensor 70 in the state transition model and an offset of the three-dimensional magnetic sensor 70 ( The change over time of (true value) may be different. On the other hand, in this embodiment, since the matrix Q _{m, k} is determined based on the temperature change amount ΔT _{m, k} , the temperature of the three-dimensional magnetic sensor 70 changes, and the state transition model changes with time of the system. The state vector x _k may be updated in consideration of the error between the change in the system expressed by the state transition model and the change in the actual system over time that occurs when It becomes possible. Thus, each element of the state vector x _k has become possible to reduce the possibility of deviation from the true value. That is, the state estimating device according to this embodiment, in the calculation of the nonlinear Kalman filter, that each element of the state vector x _k can reduce the possibility of deviation from the true value, has become possible accurate and fast state estimation .

<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
Although the state estimation apparatus according to the above-described embodiment executes the operation of the nonlinear Kalman filter using the sigma point Kalman filter, the present invention is not limited to such a form and is known as an extended Kalman filter or the like. The calculation may be performed by appropriately applying the nonlinear Kalman filter.
FIG. 4 is a functional block diagram showing functions realized when the state estimation program according to the first modification is executed. As illustrated in FIG. 4, the state estimation device according to the first modification is configured in the same manner as the state estimation device according to the embodiment except that a Kalman filter calculation unit 300 a is provided instead of the Kalman filter calculation unit 300. And the Kalman filter calculating part 300a which concerns on the modification 1 is provided with the state model part 410a instead of providing the state transition model part 410, and the point provided with the observation model part 420a instead of providing the observation model part 420, The configuration is the same as that of the Kalman filter calculation unit 300 except for.

The state transition model unit 410a calculates the estimated state vector x ⁻ _k by applying the state vector x ⁺ _k−1 to the state transition model shown in Expression (8). In addition, the state transition model unit 410a calculates the covariance Q _k of the process noise w _k based on the temperature change ΔT _{m, k} of the three-dimensional magnetic sensor 70 as shown in the equations (10) to (12). calculate.
The observation model unit 420a calculates the estimated observation value vector y ^- _k by applying the estimated state vector x ^- _k to the observation model shown in Expression (9).
Thus, in the calculation of the nonlinear Kalman filter according to the first modification, the state vector x ⁺ _k is updated without generating a plurality of sigma points.

In general, the nonlinear Kalman filter determines the Kalman gain K based on the covariance Q of the process noise w. Similar to the state estimation device according to the embodiment, the state estimation device according to Modification _{1 determines} the covariance Q _k of the process noise w _k based on the temperature change ΔT _{m, k} of the three-dimensional magnetic sensor 70. Therefore, even when the temperature change amount ΔT _{m, k of} the three-dimensional magnetic sensor 70 has a large value, it is possible to reduce the possibility that each element of the updated state vector x ⁺ _{k + 1} deviates from the true value. It becomes possible.
That is, the present invention is intended to prevent deterioration of state estimation accuracy due to a temperature change of a sensor included in the state estimation device, and is applicable regardless of the type of nonlinear Kalman filter used by the state estimation device. Is possible.

(2) Modification 2
Although the state estimation device according to the above-described embodiment and the modification estimates the system state by performing a calculation using a nonlinear Kalman filter, the present invention is not limited to this, and the linear Kalman filter It is also possible to estimate the state of the system by an operation using. That is, the state equation f and the observation equation h may be linear functions.
In general, the linear Kalman filter determines the Kalman gain K based on the covariance Q of the process noise w. Accordingly, even when the state of the system is estimated by calculation using a linear Kalman filter, the covariance Q _k of the process noise w _k is determined based on the temperature change ΔT _{m, k} of the three-dimensional magnetic sensor 70. Accordingly, even if temperature change occurs in the three-dimensional magnetic sensor 70, it becomes possible to prevent the state vector x _k is updated to a value deviated from the true value.

(3) Modification 3
In the state estimation device according to the embodiment and the modification described above, the temperature detection unit is arranged inside the sensor, but the present invention is not limited to such a configuration, and the temperature detection unit It may be arranged outside the sensor.
FIG. 5 is a block diagram illustrating a mobile device 1a according to the third modification. The portable device 1a is configured in the same manner as the portable device 1 according to the embodiment, except that the first temperature detection unit 75a is provided and a three-dimensional magnetic sensor 70a is provided instead of the three-dimensional magnetic sensor 70.
The three-dimensional magnetic sensor 70a AD-converts output signals from the X-axis magnetic sensor 71, the Y-axis magnetic sensor 72, the Z-axis magnetic sensor 73, and the magnetic sensors 71 to 73, and outputs magnetic data m. And a magnetic sensor I / F 74a. First temperature detecting section 75a detects the temperature of the three-dimensional magnetic sensor 70a, and outputs the temperature data _{T m.} That is, the 1st temperature detection part 75a functions as a temperature detection part which detects the temperature of the sensor with which the portable apparatus 1a is provided.
In the state estimation device according to the modified example 3, the first temperature detection unit 75a arranged outside the three-dimensional magnetic sensor 70a detects the temperature of the three-dimensional magnetic sensor 70a, so that the covariance Q _k of the process noise w _k is detected. Can be determined based on the temperature variation of the three-dimensional magnetic sensor 70a. Thus, it is possible to prevent the state vector x _k is updated to a value deviated from the true value.

(4) Modification 4
Embodiments and modifications described above, based on the temperature data T _m indicative of the temperature of the three-dimensional magnetic sensor 70, but were those determining the covariance Q _k of the process noise w _k, the present invention is such a form In addition to the temperature data T _m representing the temperature of the three-dimensional magnetic sensor 70, the covariance Q of the process noise w _k is considered in consideration of the temperature data T _g representing the temperature of the three-dimensional angular velocity sensor 90. _k may be determined.
FIG. 6 is a block diagram illustrating a mobile device 1b according to the fourth modification. The portable device 1b is configured in the same manner as the portable device 1 except that a three-dimensional angular velocity sensor 90a is provided instead of the three-dimensional angular velocity sensor 90.
The three-dimensional angular velocity sensor 90a is 3 except for the point provided with the second temperature detecting unit 95 that detects the temperature of the three-dimensional angular velocity sensor 90a and the point provided with the angular velocity sensor I / F 94a instead of the angular velocity sensor I / F 94. The dimensional angular velocity sensor 90 is configured similarly. The angular velocity sensor I / F 94a AD-converts output signals from the X-axis angular velocity sensor 91, the Y-axis angular velocity sensor 92, and the Z-axis angular velocity sensor 93 to output angular velocity data g, and from the second temperature detection unit 95. the output signal to the AD conversion, and outputs the temperature data T _g.
That is, in the modification 4, the 1st temperature detection part 75 and the 2nd temperature detection part 95 function as the temperature detection part 600 which detects the temperature of the sensor with which the portable apparatus 1b is provided.

In the state estimation device according to the modified example 4, the covariance Q _k of the process noise w _k is set to a value shown in the following equation (41). That is, the covariance _{Q k} of the process noise _{w k,} as shown in the following equation (41), the matrix _{Q cst, 11,} matrix _{Q cst, 12,} matrix _{Q cst, 13,} matrix _{Q cst, 23,} matrix Q _{g, k} and a matrix Q _{m, k} .
Here, the matrix Q _{cst, 11} is a 9 _× 9 symmetric matrix, and each component of the matrix Q _{cst, 11} is a constant. The matrix Q _{cst, 12} is a 9 _× 3 zero matrix, the matrix Q _{cst, 13} is a 9 _× 3 zero matrix, and the matrix Q _{cst, 23} is a 3 _× 3 zero matrix. The matrix Q _{m, k} is _{a 3 ×} 3 symmetric matrix defined by the equations (11) and (12), as in the embodiment.
The matrix Q _{g, k} is determined based on the following equations (42) and (43). Here, the amount of temperature change [Delta] T _{g, k,} as shown in the following equation (43), time T = temperature data _{T g} in _{k, k} a, the time T = temperature at k-1 data _{T g, k-} It is a value representing the absolute value of the difference from ₁ . The coefficient α _g is a positive real number. The matrix Q _{cst, g} is the process noise w _k generated in the state vector x _k in the state transition model that assumes that the offset of the three-dimensional angular velocity sensor 90a does not change even if the temperature changes in the three-dimensional angular velocity sensor 90a. It is a matrix showing the covariance of the process noise generated in the offset estimated value g _{OFF, k} of the three-dimensional angular velocity sensor 90a. In the present embodiment, each component of the matrix Q _{cst, g} is a constant.

As described above, since the state estimation device according to the modification 4 includes the temperature detection unit 600, the state vector in consideration of both the temperature change of the three-dimensional magnetic sensor 70 and the temperature change of the three-dimensional angular velocity sensor 90a. It becomes possible to update _xk .
Thus, even when the temperature of both the three-dimensional magnetic sensor 70 and the three-dimensional angular velocity sensor 90a is changed, that the elements of the state vector x _k to reduce the possibility that a value which deviates from the true value This makes it possible to accurately and quickly estimate the state.

In the fourth modification, a six-dimensional vector having the offset estimated value g _OFF of the three-dimensional angular velocity sensor 90a and the offset estimated value m _OFF of the three-dimensional magnetic sensor 70 as elements corresponds to the offset estimated vector.

In the modification 4, the temperature detection unit 600 is disposed inside the three-dimensional magnetic sensor 70 and the three-dimensional angular velocity sensor 90a, but the temperature detection unit 600 is outside the three-dimensional magnetic sensor 70 and the three-dimensional angular velocity sensor 90a. It may be arranged in.
Specifically, the first temperature detector 75 may be disposed outside the three-dimensional magnetic sensor 70, and the second temperature detector 95 is disposed outside the three-dimensional angular velocity sensor 90a. There may be.

(5) Modification 5
In the embodiment and the modification described above, the state estimation device includes the three-dimensional magnetic sensor 70, the three-dimensional acceleration sensor 80, and the three-dimensional angular velocity sensor 90. However, the present invention is not limited to such a form. Alternatively, only some of these three types of sensors may be provided. Moreover, you may provide sensors other than these three types of sensors.
That is, the state estimation apparatus according to the present invention may be any apparatus that includes a plurality of sensors that measure different physical quantities and that performs a Kalman filter operation that estimates the state of the system by integrating the outputs of the plurality of sensors.

For example, the state estimation device includes five sensors that measure different physical quantities, and among the five sensors, the offset of each of three sensors (hereinafter referred to as “first sensor group”) is an object of estimation. Assume that
In this case, the state estimation device includes a temperature detection unit that detects the temperature of each of one or more sensors (hereinafter referred to as “second sensor group”) among the three sensors constituting the first sensor group. It may be. In this example, the temperature detection unit outputs one or more temperature data corresponding to each of the one or more sensors constituting the second sensor group. Then, the state estimation device determines the covariance of process noise generated in the state transition model based on the amount of change in each of the one or more temperature data output from the temperature detection unit.
That is, in this example, a vector whose element is a value obtained by estimating the offset of each of one or more sensors constituting the second sensor group corresponds to the offset estimation vector. Then, the state estimation device includes a process noise covariance generated in the offset estimation vector among the process noise covariances defined in the state transition model, and a change amount of each of the one or more temperature data output by the temperature detection unit. Determine based on.
Accordingly, even when any temperature among the one or more sensors constituting the second sensor group have been changed, to reduce the possibility that the elements of the state vector x _k is a value which deviates from the true value Therefore, accurate and high-speed state estimation becomes possible.

In addition, although the state estimation apparatus which concerns on the modification 5 is provided with five sensors and the 1st sensor group is comprised from three sensors, this is an illustration to the last. That is, the state estimation device according to the present invention only needs to include two or more sensors, and the first sensor group includes one or more sensors among the two or more sensors included in the state estimation device. I just need it. In this case, the second sensor group may include all of the one or more sensors constituting the first sensor group, or may include a part of the one or more sensors constituting the first sensor group. It may be a thing.
Regardless of the type of sensors provided in the state estimation device and the number of sensors, the state vector _xk is updated to a value deviating from the true value by determining the covariance of the process noise based on the sensor temperature change amount. This can be prevented.

DESCRIPTION OF SYMBOLS 1 ... Portable apparatus, 10 ... CPU, 50 ... Display part, 70 ... Three-dimensional magnetic sensor, 75 ... 1st temperature detection part, 80 ... Three-dimensional acceleration sensor, 90 ... Three-dimensional angular velocity sensor, 100 ... State estimation program, 200 ... initial value generation unit, 300 ... Kalman filter calculation unit, 500 ... output information generation unit 500, m ... magnetic data, _Tm ... temperature data, ΔTm _{, k} ... temperature change amount, x ... state vector, _wk ... process noise , Q _k ... process noise covariance, y ... observation vector, z ... observation residual.

Claims (5)

A plurality of sensors including a three-dimensional magnetic sensor that detects magnetic components in three directions orthogonal to each other and sequentially outputs magnetic data expressed as vector data in a three-axis coordinate system;
A temperature detector that detects the temperature of the three-dimensional magnetic sensor and sequentially outputs temperature data;
Built into equipment with
A Kalman filter used in a state estimation device for estimating a state of a system,
A state vector having a plurality of state variables including an offset estimation vector indicating a value obtained by estimating an offset of the three-dimensional magnetic sensor,
By applying to the state transition model that represents the change of the system over time,
Estimating the state vector after elapse of unit time, and calculating this as an estimated state vector;
A state transition model part;
An observation residual is calculated based on an observation value vector having each of output values of the plurality of sensors as an element,
Updating the state vector based on the estimated state vector and the observation residual;
This is calculated as an updated state vector,
An update unit, and
When the amount of change in the temperature data in the unit time is a temperature change amount,
Of the process noise covariances defined by the state transition model,
Each component of the covariance of the process noise that occurs in the offset estimation vector is
Based on the amount of temperature change,
A Kalman filter characterized by that. Multiple sensors,
A temperature detection unit that detects a temperature of each of the one or more sensors among the plurality of sensors and outputs temperature data corresponding to each of the one or more sensors;
Built into equipment with
A Kalman filter used in a state estimation device for estimating a state of a system,
When a vector whose element is an estimated value of each of the one or more sensors among the plurality of sensors is an offset estimation vector,
A state vector including a plurality of state variables including the offset estimation vector,
By applying to the state transition model that represents the change of the system over time,
Estimating the state vector after elapse of unit time, and calculating this as an estimated state vector;
A state transition model part;
An observation residual is calculated based on an observation value vector having each of output values of the plurality of sensors as an element,
Updating the state vector based on the estimated state vector and the observation residual;
This is calculated as an updated state vector,
An update unit, and
Of the process noise covariances defined by the state transition model,
Each component of the process noise covariance that occurs in the offset estimation vector is
Based on the temperature change amount of each of the one or more temperature data in the unit time,
A Kalman filter characterized by that. The plurality of sensors include
A three-dimensional magnetic sensor that detects magnetic components in three directions orthogonal to each other and sequentially outputs magnetic data expressed as vector data in a three-axis coordinate system;
A three-dimensional angular velocity sensor that detects angular velocity around three axes orthogonal to each other and sequentially outputs angular velocity data expressed as vector data in a three-axis coordinate system;
The one or more sensors include
At least one of the three-dimensional magnetic sensor or the three-dimensional angular velocity sensor is included.
It is characterized by
The Kalman filter according to claim 2. A Kalman filter according to any one of claims 1 to 3,
The plurality of sensors, each output being an element of the observation vector;
A state estimation device comprising: a temperature detection unit that detects temperatures of one or more sensors among the plurality of sensors and outputs one or more temperature data corresponding to the one or more sensors. The temperature detector is
It is disposed inside the one or more sensors among the plurality of sensors.
The state estimation apparatus according to claim 4.

Images