JP2013185898A - State estimation device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To perform accurate state estimation by using nonlinear Kalman filters.SOLUTION: A state estimation device is embedded in a portable apparatus 1 including a plurality of sensors each for observing a system and outputting an observed value, a GPS unit 60 for outputting position information Ps, and a camera 110 for photographing an object Obj and outputting image information Img, and estimates a state of the system. The state estimation device includes: a Kalman filter operation unit 300 which includes one or more Kalman filters KF for updating a state vector x by using the state vector x including a state variable for estimating a posture μ of the portable apparatus 1 as an element and an observed value vector y having each observed value as the element; and an initial value generating unit 200 which calculates an initial value μof the posture μ of the portable apparatus 1 on the basis of the image information Img and the position information Ps, generates an initial vector INI having the initial value μas the element, and supplies this to the Kalman filter KF as an initial value of the state vector x.

Description

  The present invention relates to a state estimation device.

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

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

  In general, a nonlinear Kalman filter is composed of a state transition model that estimates changes over time in a plurality of physical quantities representing the state of a dynamic system, and a plurality of sensors that the dynamic system has based on the estimated state of the dynamic system. And an observation model for estimating a value to be measured (observed value). Then, the nonlinear Kalman filter uses the difference (observation residual) between the estimated observation value and the observation value actually measured by the plurality of sensors to estimate each of the plurality of physical quantities representing the state of the dynamic system. 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

  By the way, since the nonlinear Kalman filter is an operation that sequentially updates the state vector using the state vector and the observation residual calculated based on the state vector, each element of the initial value of the state vector deviates from the true value. If the value is set to the specified value, it may take a long time for each element of the state vector to converge to a value close to the true value. Convergence can also occur. In this case, the nonlinear Kalman filter cannot accurately estimate the state of the system.

  The present invention has been made in view of the above-described points, and an object of the present invention is to perform accurate state estimation using a nonlinear Kalman filter.

  In order to solve the above-described problems, a state estimation apparatus according to the present invention includes a plurality of sensors that observe a system and output observation values, a position information output unit that outputs position information, and image information obtained by photographing an object. A state estimation device that estimates a state of the system, and is a state vector having a plurality of state variables as elements to estimate the state of the system; , Using an observation value vector having each of the observation values as an element, a Kalman filter operation unit including one or more Kalman filters for updating the state vector, and generating an initial value that is an initial value of the state vector And the plurality of state variables include state variables for estimating the posture of the device, and the initial value generation unit includes one or more objects as the image acquisition unit. Initial posture estimation, which is an initial value of a state variable for estimating the posture of the device, based on the image information output by the image acquisition unit when the image is taken and the position information output by the position information output unit A value is calculated, and the initial vector having the initial posture estimated value as an element is generated.

Since the posture of the device depends on the operation of the user of the device, if the initial posture estimated value is set to some predetermined fixed value, the initial posture estimated value and the true posture of the device are large. There may be a gap.
According to this invention, the initial posture estimated value is calculated based on the image information and the position information. Based on the image information, the direction of the object viewed from the device can be calculated. Therefore, when the positional relationship between the device and the object can be calculated based on the position information representing the position of the device, the orientation of the device is determined based on the direction of the object viewed from the device and the positional relationship between the device and the object. Can be calculated. As a result, it is possible to prevent the state variable for estimating the posture of the device from greatly deviating from the true posture of the device, and the state estimation device can accurately estimate the state of the system.

  Further, in the state estimation device described above, the Kalman filter calculation unit includes a predetermined number of the Kalman filters that operate in parallel, and the initial value generation unit includes the image information obtained by the image acquisition unit capturing a single object. In a device coordinate system, which is a three-axis coordinate system fixed to the device, based on the image information, a first direction vector representing the direction of the one object viewed from the device is A second direction vector representing the direction of the first object viewed from the device in a ground coordinate system, which is a three-axis coordinate system fixed on the earth, based on the image processing unit to be calculated and the position information; And calculating an initial posture candidate value based on the first direction vector and the second direction vector, and rotating the initial posture candidate value by a predetermined angle about the second direction vector as a rotation axis. Generating the predetermined number of the initial posture estimation values, and generating the predetermined number of the initial vectors corresponding to each of the predetermined number of the initial posture estimation values. It is preferable that an initial vector generation unit respectively supplied to the predetermined number of the Kalman filters.

According to the present invention, the initial value generation unit calculates a predetermined number of initial posture estimation values, and generates a predetermined number of initial vectors corresponding to each of the predetermined number of initial posture estimation values. Then, the initial value generation unit supplies the predetermined number of initial vectors to the predetermined number of Kalman filters.
The predetermined number of initial vectors indicate different values. Each of the predetermined number of initial vectors is calculated using a first direction vector determined based on the image information and a second direction vector determined based on the position information. Therefore, the predetermined number of initial vectors includes initial vectors in which each element has a value that does not greatly deviate from the actual physical quantity. The Kalman filter supplied with the initial vector can accurately estimate the state of the system. Thereby, the state estimation apparatus according to the present invention can accurately estimate the state of the system.

  In the state estimation device described above, the Kalman filter calculation unit includes one Kalman filter, and the initial value generation unit is configured when the image acquisition unit captures two objects and outputs the image information. Based on the image information, in a device coordinate system that is a three-axis coordinate system fixed to the device, two first direction vectors representing the directions of the two objects viewed from the device are calculated. And a second coordinate system representing the directions of the two objects viewed from the device in a ground coordinate system that is a three-axis coordinate system fixed on the earth based on the position information. An initial posture calculation unit that calculates a two-direction vector, calculates the initial posture estimation value based on the two first direction vectors and the two second direction vectors, and requires the initial posture estimation value. Generating the initial vector to which a and a initial vector generator to be supplied to the Kalman filter, it is preferable.

  According to this invention, since the initial posture estimated value is calculated based on the two first direction vectors and the two second direction vectors, the initial posture estimated value is a value that accurately represents the actual posture of the device. . Therefore, the state estimation apparatus according to the present invention can perform accurate state estimation.

  Further, the state estimation device described above includes a time output unit that outputs time information, and the initial posture calculation unit calculates the second direction vector based on the position information and the time information. It is preferable.

The object according to the present invention corresponds to celestial bodies such as the sun and the moon. The celestial body can specify the direction of the object viewed from the device in the ground coordinate system based on the position information and the time information. That is, the second direction vector calculated based on the position information and the time information accurately represents the direction of the object viewed from the device.
Since the state estimation device calculates the initial posture estimation value based on such a second direction vector, it is possible to perform accurate state estimation.

  In the state estimation device described above, the object has known coordinates in the ground coordinate system, and the initial posture calculation unit is based on the position information and the coordinates of the object in the ground coordinate system. It is preferable that the second direction vector is calculated.

The object according to the present invention corresponds to a landmark whose coordinates in the ground coordinate system are known among landmarks such as famous buildings. An object whose coordinates in the ground coordinate system are known can specify the direction of the object viewed from the device based on the coordinates and the position information of the device. That is, the second direction vector calculated based on the position information and the coordinates of the object in the ground coordinate system accurately represents the direction of the object viewed from the device.
Since the state estimation device calculates the initial posture estimation value based on such a second direction vector, it is possible to perform accurate state estimation.

It is a block diagram which shows the structure of the portable apparatus which concerns on embodiment of this invention. It is a perspective view which shows the external appearance of a portable apparatus. It is a functional block diagram showing the function of a state estimation apparatus. It is explanatory drawing showing the relationship between the attitude | position of a portable apparatus, and an object. It is a functional block diagram showing the function of a Kalman filter calculating part. It is a functional block diagram showing the function of the initial value generation part which concerns on the modification 1. FIG. It is a functional block diagram showing the function of the state estimation apparatus which concerns on the modification 5. It is a functional block diagram showing the function of the Kalman filter operation part concerning the modification 6.

<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 (device) rotates the image such as a map displayed on the screen included in the mobile device 1 in accordance with the change in the posture of the mobile device 1, thereby changing the orientation represented by the image to the orientation in the real space. It has a function to follow. 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 geomagnetism viewed from the mobile device 1.

The portable device 1 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, a state estimation program 100, and various programs and various setting information. The stored ROM 30, the communication unit 40 that executes communication, the display unit 50 that displays images, and information from GPS satellites (specifically, information about the latitude and longitude of the mobile device 1) are acquired and output. A GPS unit 60, a camera 110 (image acquisition unit) that captures an object Obj existing outside the mobile device 1 and outputs image information Img, an operation unit 120 used for various inputs of a user of the mobile device 1, and A clock 130 (time output unit) that outputs time information Time is provided.
The portable device 1 detects a magnetic field such as geomagnetism and outputs magnetic data q, a three-dimensional acceleration sensor 80 that detects acceleration and outputs acceleration data a, and detects an angular velocity. A three-dimensional angular velocity sensor 90 that outputs angular velocity data g is provided.
In the present embodiment, various programs and various setting information are stored in the ROM 30, but some or all of the various programs and various setting information may be stored in the RAM 20.

The display unit 50 displays the orientation of geomagnetism calculated based on the state of the system estimated by the CPU 10 executing the state estimation program 100, the posture of the portable device 1, and the like as images such as arrows. The display unit 50 may display an image based on the image information Img when the camera 110 captures the object Obj and outputs the image information Img on which the object Obj is drawn.
The GPS unit 60 receives a signal from a GPS satellite and outputs position information Ps (latitude, longitude) of the mobile device 1. That is, the GPS unit 60 functions as a “position information output unit” that acquires the position information Ps of the mobile device 1 and outputs the position information Ps.
The operation unit 120 is used by the user of the mobile device 1 to input an instruction for photographing the object Obj using the camera 110.

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 performs AD conversion on 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 q. The magnetic data q is vector data represented by three components of the x axis, the y axis, and the z axis. More specifically, the magnetic data q is an output value from the X-axis magnetic sensor 71 in a three-axis orthogonal coordinate system fixed to the portable device 1 (strictly, fixed to the three-dimensional magnetic sensor 70). This is vector data expressed as an axis component, an output value from the Y-axis magnetic sensor 72 is expressed as a y-axis component, and an output value from the Z-axis magnetic sensor 73 is expressed as a z-axis component.
Hereinafter, the three-axis orthogonal coordinate system fixed to the mobile device 1 is referred to as “device coordinate system Σ _S ”. The coordinate system fixed to the three-dimensional magnetic sensor 70, the coordinate system fixed to the camera 110, the coordinate system fixed to the three-dimensional acceleration sensor 80, and the coordinate system fixed to the three-dimensional angular velocity sensor 90 are: strictly speaking, it has an origin at different positions, respectively, in the following description, may be collectively referred to as device coordinate system sigma _S.

Incidentally, the magnetic data q 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 in a three-axis orthogonal coordinate system (hereinafter referred to as “ground coordinate system Σ _G ”) fixed at a certain point on the ground. , it expressed as a vector ^G Bg having a constant direction and magnitude.
Here, 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. Note that the origin of the ground coordinate system sigma _G, and the three axes of the orientation orthogonal to each other with the ground coordinate system sigma _G, it may be how determined. In the present embodiment, the ground coordinate system sigma _G, oriented magnetic poles north y-axis, z-axis defined as facing upward vertical.
Thus, when expressed in the device coordinate system sigma _S, geomagnetism Bg changes the direction in conjunction with the portable device 1 posture mu, expressed as a constant magnitude of the vector ^S Bg (mu) (Note that the vector accompanied shown in the upper left of a code letter "S", the vector means that the vector represented by the device coordinate system sigma _S). Therefore, based on the vector ^S Bg (mu) representing the geomagnetism Bg in the device coordinate system sigma _S, it can be determined orientation of the portable device 1 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 q) 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 mobile device 1. It becomes a vector. Therefore, to calculate a vector ^S Bg (mu) representing the geomagnetism Bg in equipment coordinate system sigma _S, in order to obtain the geomagnetism Bg direction as viewed from the portable device 1 correctly, the device 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 portable device 1 from the coordinates indicated by the magnetic data q output from the sensor is required.
In this way, 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 that is removed from the magnetic data q 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 is obtained from the resultant force of inertial force and gravity in the device coordinate system Σ _S (strictly, the coordinate system fixed to the three-dimensional acceleration sensor 80), and the three components of the x-axis, y-axis, and z-axis. It is data shown as a vector having. Accordingly, the portable device 1 if stationary or constant velocity linear motion state, the acceleration data a is a vector data indicating the magnitude and direction of gravitational acceleration in the device coordinate system sigma _S.

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 of the device coordinate system Σ _S (strictly, the coordinate system fixed to the three-dimensional angular velocity sensor 90).

  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 device.
As shown in FIG. 3, the state estimation apparatus executes an operation of a nonlinear Kalman filter in parallel using an initial value generation unit 200 that outputs a plurality of different initial vectors INI and a plurality of Kalman filters that operate in parallel. A Kalman filter operation unit 300 that outputs a state vector x, an output information control unit 400 that selects and outputs one state vector x from a plurality of state vectors x output by the Kalman filter operation unit 300, and an output information control unit 400 Is provided with an output information generation unit 500 that generates output information based on one state vector x output from the.

In the present embodiment, the Kalman filter unit 300 includes four Kalman filters KF. Each Kalman filter KF performs a nonlinear Kalman filter operation.
Although the details will be described later, the calculation of the nonlinear Kalman filter is an observation that is a vector having as elements the state vector x that represents the state of the system estimated by the state estimation device and the output value from the sensor provided in the state estimation device. This is an operation for estimating the state of the system by periodically updating the state vector x based on the observation residual z calculated using the value vector y. The state vector x has an initial vector INI as an initial value.
Hereinafter, for the sake of convenience of explanation, when the four Kalman filters KF are distinguished, each of the four Kalman filters is represented as KF [1], KF [2], KF [3], and KF [4]. Of the four Kalman filters, the m-th (m is a natural number satisfying 1 ≦ m ≦ 4) Kalman filter may be expressed as KF [m].
Each of the four Kalman filters KF outputs a state vector x calculated by executing a nonlinear Kalman filter operation and an observation residual z.

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 physical quantity indicating 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 q _OFF of the three-dimensional magnetic sensor 70 are adopted, and these are used as estimation targets in the nonlinear Kalman filter calculation.
The state vector x _{k at} time T = k is expressed by the following equation (1). The subscript “k” attached to the lower right of each state variable indicates that the state variable is a value at time T = k. In the following description, the subscript “k” may be added to the lower right of various variables, vectors, and matrices to indicate that the variables, vectors, matrices, etc. are values 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 q _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, a posture μ in which each axis of the device coordinate system Σ _S and each axis of the ground coordinate system Σ _G coincide is defined as a “reference posture”, and the reference posture is μ = (0, 0, 0, 1). Express. At this time, an arbitrary posture μ of the mobile device 1 can be expressed as a posture rotated counterclockwise by an angle ψ when viewed from the direction indicated by the unit vector ρ with the unit vector ρ as a rotation axis. The posture μ is expressed by Equation (2) using a quaternion.

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

Each Kalman filter KF executes a nonlinear Kalman filter operation and periodically updates the state vector x so that each element of the state vector x of the Kalman filter KF has a true value, thereby representing a plurality of system states. Estimate the physical quantity of.
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.
For convenience of explanation, in the following description, the code [m] may be attached to the state vector x _k of the mth Kalman filter KF [m]. Further, various matrices, vectors, and the like used by the m-th Kalman filter KF [m] in the calculation of the nonlinear Kalman filter may be expressed with the symbol [m].
Although details will be described later, in this embodiment, the state vector x _k updated by each Kalman filter KF is strictly the updated state vector x ⁺ _k .

The initial value generation unit 200 outputs setting information stored in the ROM 30 (specifically, information Pobj described later, information in which position information is associated with geomagnetic strength and inclination), and the GPS unit 60 outputs the setting information. Based on the position information Ps, the image information Img output from the camera 110, and the like, four different initial vectors INI [1] to INI [4] are generated and output.
The four initial vectors INI [1] to INI [4] are supplied to the four Kalman filters KF [1] to KF [4], respectively, and are used when each Kalman filter KF executes the operation of the nonlinear Kalman filter. Adopted as the initial value of the state vector x. That is, the initial vector INI [m] is the initial value of the state vector x _k , that is, the state vector x ₀ at time T = 0.

The initial vector INI [m] supplied to the m-th Kalman filter KF [m] is an initial value μ ₀ [m] (initial posture estimated value) of the posture μ [m] of the mobile device 1, and the geomagnetism strength r. Initial value r ₀ [m] of [m], initial value φ ₀ [m] of geomagnetic dip angle φ [m], initial value ω ₀ [m] of angular velocity ω [m] of portable device 1, three-dimensional angular velocity sensor The initial value g _{OFF, 0} [m] of the 90 estimated offset value g _OFF [m] and the initial value q _{OFF, 0} [m] of the estimated offset value q _OFF [m] of the three-dimensional magnetic sensor 70 are included as elements. . This initial vector INI [m] is expressed by the following equation (4).
A method for generating each element of the initial vector INI [m] will be described later.

The initial value generation unit 200 includes an image processing unit 210, an initial posture calculation unit 220, and an initial vector generation unit 230.
The image processing unit 210, based on image information Img output from the camera 110, and calculates the first direction vector α representing the direction of the object Obj viewed from the portable device 1 in the device coordinate system sigma _S. The initial posture calculation unit 220 is based on the position information Ps output from the GPS unit 60, the information Pobj stored in the ROM 30, and the first direction vector α output from the image processing unit 210, and the initial value μ _{0 of the} posture μ. [1] to μ ₀ [4] (initial posture estimated value) are calculated. The initial vector generation unit 230 includes initial values μ ₀ [1] to μ ₀ [4] of the posture μ calculated by the initial posture calculation unit 220 and setting information (specifically, position information and geomagnetism) stored in the ROM 30. The initial vectors INI [1] to INI [4] are calculated on the basis of the information obtained by associating the strength and the dip angle with each other.

The output information control unit 400 has the highest estimation accuracy among the four Kalman filters KF [1] to KF [4] based on the output values from the four Kalman filters KF [1] to KF [4]. Select the Kalman filter KF [Sel]. Then, the output information control unit 400 supplies the output information generation unit 500 with the state vector x _k [Sel] output from the selected Kalman filter KF [Sel].

The estimation accuracy of the Kalman filter KF is an index representing how accurately the Kalman filter KF estimates the state of the system. Specifically, the value indicated by the state vector x _k and the true value of the state vector This is a value (error evaluation value) based on the magnitude of the error. Then, the estimation accuracy of the Kalman filter KF is the high state, the error between the true value of the values and the state vector indicating the state vector x _k is small (i.e., smaller error evaluation value) means that the state.
When the error between the value indicated by the state vector x _k and the true value of the state vector is small (that is, when each element of the state vector x _k indicates an accurate value close to the true value), the Kalman filter having the state vector x _k KF can be regarded as accurately estimating the system.

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

In the present embodiment, the maximum value of the norm of the observation residual z _k [m] in a certain period is adopted as the error evaluation value ^MAX z _k [m]. However, the present invention is limited to such a form. Instead, the maximum value in a predetermined period of the norm of a vector composed of some of the plurality of elements constituting the observation residual z _k [m] may be adopted as the error evaluation value.
Although details will be described later, the observation residual z _k [m] according to the present embodiment is configured by an element corresponding to the magnetic data q, an element corresponding to the acceleration data a, and an element corresponding to the angular velocity data g. Vector (see equation (52)). In this case, the peak monitor PM [m] monitors, for example, elements corresponding to the magnetic data q among elements constituting the observation residual z _k [m] for a predetermined period, and sets the maximum norm of the element as an error. it may be used as the evaluation value ^MAX z _k [m].

The comparison unit 410 has a minimum error evaluation value ^MAX z among the error evaluation values ^MAX z _k [1] to ^MAX z _k [4] output by the four peak monitors PM [1] to PM [4]. _The selected value Sel indicating the Kalman filter KF [Sel] corresponding to _k [Sel] is output (where Sel is a natural number satisfying 1 ≦ Sel ≦ 4).
As described above, the observation residual z is a value calculated using the state vector x and the observation value vector y. Based on the observation residual z, the value indicated by the state vector x, the true value of the state vector, The magnitude of the error can be expressed. However, the value indicated by each element of the observed value vector y is a value on which noise (for example, measurement errors of various sensors) is superimposed, and is a value that varies with the passage of time. Therefore, the value indicated by each element of the observation residual z is also a value superimposed with noise that varies with the passage of time.
Therefore, the comparison unit 410 according to the present embodiment monitors the observation residual z _k [m] for a predetermined period, and determines the maximum norm of the observation residual z _k [m] in the predetermined period as the error evaluation value ^MAX z _k [ m], and the error evaluation value ^MAX z _k [m] is used to evaluate the magnitude of the error between the value indicated by the state vector x _k and the true value of the state vector. Thus, comparing unit 410 according to this embodiment, by reducing the effect of noise, the magnitude of the error between the true value of the values and the state vector indicating the state vector x _k can be accurately evaluated.

Selecting unit 420, based on the selection value Sel the comparison unit 410 outputs, from the Kalman filter KF [1] the state vector _x k [1] ～KF to [4] is output _~x k [4], the state vector _{x k} [Sel] is selected and supplied to the output information generation unit 500.

The output information generation unit 500 calculates output information such as the orientation of the geomagnetism Bg and the attitude of the mobile device 1 based on the state vector x _k [Sel] output from the output information control unit 400.

[2. Initial vector generation]
As described above, the calculation of the nonlinear Kalman filter executed by each Kalman filter KF is an operation of updating the state vector x based on the observation residual z. More specifically, the calculation of the nonlinear Kalman filter is an observation residual z _k calculated based on the state vector x _k−1 at time T = k ₋₁ and the observation value vector y _{k at} time T = k. Is used to estimate the state vector x _k at time T = k. That is, the state vector x _k at time T = k is calculated based on the state vector x _k−1 at time T = k−1. In other words, the state vector _{x k} is calculated based on the initial state (time T = 0) at time T = all of the state vector _x leading to _{k-1 0 ~x k-1} .
In the calculation of the nonlinear Kalman filter, when each element of the state vector x _k is updated to a value deviating from the true value, it takes a long time until each element of the state vector x _k converges to a value close to the true value. In addition, each element of the state vector x _k may converge to an inaccurate value different from the true value. In this case, the state estimation device cannot accurately estimate the state of the system. Therefore, in order for the state estimation device to accurately and quickly estimate the state of the system, each element of the initial vector INI, which is the initial value of the state vector x _k (state vector x ₀ ), is an accurate value close to the true value. It is desirable to set to.
Therefore, the initial value generation unit 200 according to the present embodiment is configured so that each element of at least one initial vector INI out of the initial vectors INI [1] to INI [4] has an accurate value close to a true value. Initial vectors INI [1] to INI [4] are generated. As a result, the state of the system can be accurately estimated by at least one Kalman filter KF among the four Kalman filters KF [1] to KF [4].
Hereinafter, generation of initial vectors INI [1] to INI [4] in the initial value generation unit 200 will be described in detail.

[2.1. Initial posture values]
First, the initial value μ ₀ [m] of the attitude μ [m] of the mobile device 1 in the initial vector INI [m] is examined.
The posture μ of the mobile device 1 is determined depending on how the user of the mobile device 1 holds the mobile device 1. Accordingly, suppose that the initial value μ ₀ [m] of the posture μ [m] is set to one fixed value (for example, μ ₀ [m] = [ ₀ , ₀ , ₀ , 1] (reference posture)). ), The initial value μ ₀ [m] is shifted from the true posture (true value) by 180 degrees at the maximum. When the initial value μ ₀ [m] deviates greatly from the true posture (true value) and the initial vector INI [m] does not accurately represent the system state, the state estimation device accurately estimates the system state. It is not possible.
Therefore, in this embodiment, by using the image information Img and the position information Ps, the initial value _{μ 0 [1] ~μ 0 [} 4] at least one exact value close initial value mu ₀ is the true value of the The initial values μ ₀ [1] to μ ₀ [4] (initial posture estimated values) are generated so that
Hereinafter, detailed explanation of the method of generating the initial value _{μ 0 [1] ~μ 0 [} 4].

First, the image processing unit 210 calculates the first direction vector α based on the image information Img output when the camera 110 captures the object Obj.
Here, the first direction vector alpha, in the device coordinate system sigma _S, the portable device 1 (strictly speaking, the camera 110) is a three-dimensional unit vector representing the direction of the object Obj viewed from. Below, the detail of the calculation method of the 1st direction vector (alpha) is demonstrated.

The camera 110 includes, for example, one or more lenses and a plurality of image sensors arranged in a matrix (or honeycomb) on a light receiving plane that is a plane perpendicular to the optical axis of the one or more lenses. The camera 110 focuses the light beam emitted by the object Obj (or the light beam reflected by the object Obj) on a partial area on the light receiving plane by passing through the lens. Next, the camera 110 detects the brightness of each position on the light receiving plane using a plurality of image sensors. The camera 110 composes an image based on the output values of the plurality of image sensors, and generates image information Img that is the position (two-dimensional coordinates) of the object Obj in the image.
In the present embodiment, the generation of the image information Img is performed by the camera 110, but may be performed by the image processing unit 210 illustrated in FIG. In addition, the display part 50 can display the image in which the object Obj was reflected (refer FIG. 2). Accordingly, the user of the mobile device 1 may generate the image information Img by specifying the object Obj shown in the image via the operation unit 120.

Here, for the sake of simplicity, taking a convex lens as an example, the movement of light passing through the lens will be described.
As a general property of a lens, light passing through the center of the lens travels straight without changing its direction after passing through the lens. The light passing through the focal point of the lens becomes light parallel to the optical axis of the lens after passing through the lens. On the other hand, light parallel to the optical axis of the lens passes through the focal point of the lens after passing through the lens. By utilizing the optical properties of such a lens, the incident light seen from the lens is incident on the basis of the imaging position when the light emitted from the point light source is condensed at one point (image point) by the lens. The direction can be calculated.
More specifically, in an ideal convex lens, the intersection of the light receiving plane and the straight line representing the path of light passing through the center of the lens is the image point, so the vector representing the center of the lens from the image point and the light The vector representing the incident direction is parallel. Further, since the image information Img is the position of the image point on the light receiving plane, a vector representing the center of the lens from the image point can be obtained based on the image information Img. Therefore, the incident direction of the light can be obtained based on the image information Img.
Further, even when the lens aberration is taken into account, or even when the camera 110 includes a plurality of lenses, the position of the image point on the light receiving plane and the incident direction of light incident on the lens are 1: 1. Corresponding to Therefore, even if the vector representing the center of the lens from the image point and the vector representing the incident direction of the light are not parallel, the image point on the light receiving plane is stored on the ROM 30 or RAM 20. By providing a lookup table that stores the position (image information Img) and the incident direction of light incident on the lens in association with each other, the incident direction of the light viewed from the center of the lens based on the image information Img It is possible to obtain a vector representing.
Note that the image information Img may include the distance between the center of the lens and the light receiving plane in addition to the position of the image point on the light receiving plane. In addition to the position of the image point on the light receiving plane, the distance between the light receiving plane and the lens is included in the image information Img, so that even if the light receiving plane is moved away from or closer to the lens, it is based on the image information Img. The incident direction of light viewed from the lens can be calculated.
In this manner, the direction of the object Obj viewed from the camera 110 can be calculated based on the image information Img output when the object Obj is captured by the camera 110. The optical axis of the lens faces a fixed direction in the instrument coordinate system sigma _S. Accordingly, the image processing unit 210, based on image information Img, direction of the object Obj viewed from the portable device 1, i.e., it is possible to calculate the first direction vector α expressed in device coordinates sigma _S.

As shown in FIG. 3, the initial posture calculation unit 220 includes a shooting target information acquisition unit 221 and a posture calculation unit 222. Photographing object information acquisition unit 221 calculates the position information Ps of GPS unit 60 outputs the coordinates of the object Obj of the ground coordinate system sigma _G, the second direction vector β based on. The posture calculation unit 222 calculates initial values μ ₀ [1] to μ ₀ [4] of the posture μ of the mobile device 1 based on the first direction vector α and the second direction vector β.
Here, the second direction vector beta, in ground coordinate system sigma _G, a three-dimensional unit vector representing the direction of the object Obj viewed from the portable device 1.

In the present embodiment, the object Obj is coordinates in ground coordinate system sigma _G is a known landmark. Here, the landmark is a building that exists in various parts of Japan (or around the world), or a landform that exists in various parts of Japan (or around the world). The landmark may be a light source provided in a building or the like.
The ROM 30 stores a lookup table LUT1 that stores information Pobj about each of a plurality of landmarks that can be candidates for the object Obj. Here, the specific content of the information Pobj is information such as longitude, latitude, and altitude for specifying the position of each landmark. Note that the information Pobj may include information regarding the shape of each landmark. In this embodiment, the information Pobj is stored in the lookup table LUT1 stored in the ROM 30, but the information Pobj is acquired from the outside of the portable device 1 via the communication unit 40 every time and stored in the RAM 20. May be.
The imaging target information acquisition unit 221 refers to the lookup table LUT1 and acquires information Pobj about the object Obj captured by the camera 110. The imaging target information acquiring unit 221, based on the information Pobj, calculates the coordinates of the object Obj of the ground coordinate system sigma _G, based on the position information Ps of the portable device 1, a mobile in the ground coordinate system sigma _G The coordinates of the device 1 are calculated. Thereafter, photographing object information acquiring unit 221, from the coordinates of the object Obj of the ground coordinate system sigma _G, by subtracting the coordinates of the portable device 1 in the ground coordinate system sigma _G, to calculate the second direction vector beta.
In the present embodiment, the position information Ps includes the latitude and longitude of the mobile device 1, but the position information Ps may include the altitude of the mobile device 1. In this case, the second direction vector β can be calculated more accurately.

In the present embodiment, the user of the mobile device 1 selects an object Obj actually captured by the camera 110 from a plurality of landmarks.
However, the present invention is not limited to such an embodiment, and the imaging target information acquisition unit 221 may identify the object Obj actually captured by the camera 110 based on the position information Ps and the information Pobj. Good.
Further, when the CPU 10 executes the state estimation program 100, the CPU 10 is based on the position information Ps and the information Pobj, and the object Obj (for example, a portable object) that can be photographed by the user of the portable device 1 from a plurality of landmarks. A message that prompts the user to select a landmark present in the vicinity of the device 1 and shoot the object Obj may be displayed on the display unit 50. In this case, the CPU 10 may calculate the direction of the object Obj viewed from the user based on the position information Ps and the information Pobj and display it on the display unit 50.

Next, the posture calculation unit 222 calculates initial values μ ₀ [1] to μ ₀ [4] of the posture μ of the mobile device 1 based on the first direction vector α and the second direction vector β.
Hereinafter, a method for generating the initial values μ ₀ [1] to μ ₀ [4] based on the first direction vector α and the second direction vector β will be specifically described.

As described in paragraph [0032], an arbitrary posture of the object can be expressed as a posture rotated by an angle ψ with the unit vector ρ as a rotation axis. Further, a rotation matrix R (ρ, ψ) of 3 rows and 3 columns that is rotated counterclockwise by an angle ψ when viewed from the direction indicated by the unit vector ρ, with the unit vector ρ as a rotation axis is expressed by the following equation (6) Expressed (Rodriguez's formula). Therefore, an arbitrary posture of the object can be expressed by using the rotation matrix R (ρ, ψ) instead of the quaternion. Here, [ρ ×] appearing in Equation (6) is a 3 × 3 matrix, and when the unit vector ρ is expressed by Equation (7), [ρ ×] is expressed by Equation (8).
When the posture of the mobile device 1 is the reference posture, the device coordinate system Σ _S and the ground coordinate system Σ _G coincide with each other, while the posture of the mobile device 1 is represented by a rotation matrix R (ρ, ψ). the relationship between the device coordinate system sigma _S and ground coordinate system sigma _G is also represented by the rotation matrix R (ρ, ψ). Specifically, if representing the orientation of the portable device 1 in the rotation matrix R (ρ, ψ), the rotation matrix R (ρ, ψ) is, x-axis of the ground coordinate system sigma _G, y-axis, and z-axis the three unit vectors expressed from the device coordinate system sigma _S, respectively, first column, second column, a matrix arranged in the third column.

In the following, when the posture of the mobile device 1 is expressed by a rotation matrix, the symbol “A” is used like “posture A”.
Here, as shown in Expression (9), a rotation matrix R (ρ _C , _C) that rotates the reference posture counterclockwise by an angle ψ _C when viewed from the direction indicated by the unit vector ρ _C with the unit vector ρ _C as a rotation axis. Let the posture represented by (ψ _C ) be the posture A _C (initial posture candidate value). Then, it is assumed that the orientation of the mobile device 1 when the A _C, the first direction vector α and the second direction vector β is calculated.
In this case, the unit vector ρ _C is expressed by Expression (10) using the first direction vector α and the second direction vector β. The angle [psi _C, using the first direction vector α and the second direction vector beta, represented by the formula (11).

As described above, the rotation matrix R (ρ, ψ) is in the device coordinate system sigma _S, the three unit vectors representing the three-axis terrestrial coordinate system sigma _G is a matrix arranged in each column. Therefore, the rotation matrix R (ρ, ψ) may have any three-dimensional vector expressed in ground coordinate system sigma _G, also property of the coordinate transformation matrix for representing the device coordinate system sigma _S.
Therefore, as shown in the following equation (12), the second direction vector β expressed in ground coordinate system sigma _G, by converting the rotation matrix R ([rho, [psi), the object viewed from the portable device 1 Obj direction, i.e., it is possible to calculate the first direction vector α expressed in device coordinates sigma _S.

By the way, even if the first direction vector α and the second direction vector β are calculated, the posture of the mobile device 1 cannot be uniquely specified. That is, the equation (9) to (11), and calculate the position A _C based on the first direction vector α and the second direction vector beta, the posture A _C is exactly the attitude of the mobile device 1 There is a high possibility of not expressing.
FIG. 4A is a conceptual diagram illustrating the positional relationship between the mobile device 1 and the object Obj. In FIG. 4 (A), the convenience, as the origin of the ground coordinate system sigma _G, and the origin of the instrument coordinate system sigma _{S (instrument} coordinate system sigma _S1, instrument coordinate system sigma _S2) coincide, ground coordinate system Assume that Σ _G and device coordinate system Σ _S are provided.
As shown in FIG. 4 (A), when the orientation of the mobile device 1 is A _C1, as an axis a line AX connecting the portable device 1 and the object Obj, rotated the device by an arbitrary angle θ attitude _{Is AC2} . At this time, the direction of the object Obj viewed from the device coordinate system sigma _S1 fixed to the portable device 1 in position A _C1, from the device coordinate system sigma _S2 fixed to the portable device 1 in position A _C2 of the object Obj viewed The direction matches. That is, the first direction vector α calculated when the attitude of the mobile device 1 is A _C1 and the first direction vector α calculated when the attitude of the mobile device 1 is A _C2 are equal.
Further, the second direction vector beta, is a vector represented in ground coordinate system sigma _G. Therefore, even if the attitude of the mobile device 1 changes, the second direction vector β is constant as long as the position of the mobile device 1 (position information Ps) does not change. That is, the second direction vector β calculated when the attitude of the mobile device 1 is A _C1 and the second direction vector β calculated when the attitude of the mobile device 1 is A _C2 are equal.
In this case, even if the posture A _C1 is calculated based on the first direction vector α and the second direction vector β, the true posture of the mobile device 1 may be the posture _AC2 . That is, the posture of the mobile device 1 when satisfied Equation (12) when the _{A C1,} also expression when the portable device 1 of the posture _{A C2} (12) is satisfied.

Here, as shown in FIG. 4 (B), the portable device 1 in the second direction vector beta (linear AX) rotation matrix rotated by an arbitrary angle theta as the axis R (β, θ) using the attitude A _C Consider rotating the.
As described above, when the posture of the mobile device 1 is satisfied Equation (12) when the A _C, even when rotated by the posture A _C rotation matrix R of the portable device 1 (beta, theta) formula (12 ) Holds. That is, when Expression (12) holds, the following Expression (13) also holds simultaneously.
Here, as shown in equation (14), the portable device 1 in position A _C rotation matrix R (beta, theta) a posture is rotated by a "A". In this case, as shown in Expression (15), the posture A represents a general solution of the posture of the mobile device 1 when the first direction vector α and the second direction vector β are calculated.

As shown in FIG. 4B, the posture calculation unit 222 according to the present embodiment rotates the posture _AC by (π / 2) about the straight line AX (second direction vector β) as a posture. A [1] to A [4] are generated. Specifically, as shown in Expression (16), the posture when the angle θ is set to “0” is A [1]. A posture A [1], equal to the position _{A C.} Next, as shown in Expression (17) to Expression (19), the angle θ is set to “π / 2”, “π”, and “(3π) / 2”, respectively, and the three postures A [2 ], A [3], A [4].
Then, the posture calculation unit 222, a posture A [1] ~A [4] expressed by a matrix, by converting the quaternion, to generate an initial value _{μ 0 [1] ~μ 0 [} 4].

Note that the conversion from the posture A represented by the matrix of 3 rows and 3 columns to the posture μ represented by the quaternion may be performed using a known method as appropriate.
Here, when the portable device 1 is attitude mu, a vector expressed in ground coordinate system sigma _G, matrix B for performing coordinate transformation for representing by the device coordinate system Σ _S (μ) has the following formula (20). Since the matrix A is an inverse matrix of the matrix B (μ), by comparing each component of the matrix A and each component of the inverse matrix of the matrix B (μ), the posture μ represented by the quaternion is expressed. Each element may be calculated. Further, for example, the posture μ can be calculated from the matrix A by using a method described in the following publicly known document.
Malcolm D. Shuster and Gregory A. Natanson, “Quaternion Computation from a Geometric Point of View”, The Journal of the Astronautical Sciences, Vol. 41, No. 4, October-December 1993, pp. 545-556.

As described above, the initial posture calculation unit 220 according to the present embodiment uses the image information Img and the position information Ps, and at least one of the initial values μ ₀ [1] to μ ₀ [ 4] can be generated.

[2.2. Regarding values other than the initial posture values]
The initial value r ₀ [m] of the geomagnetic strength r [m] and the initial value φ ₀ [m] of the geomagnetic depression angle φ [m] are based on, for example, the position information Ps supplied from the GPS unit 60. Can be generated. 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 LUT2 that stores position information, geomagnetic strength, and dip angle in association with each other. The initial value generation unit 200 refers to the lookup table LUT2 and determines the geomagnetism strength and dip angle corresponding to the position information Ps, the initial value r ₀ [m] of the geomagnetism strength r [m], and the geomagnetic dip angle. Set as the initial value φ ₀ [m] of φ [m].

The initial value ω ₀ [m] of the angular velocity ω [m] is set to “0” on the assumption that the mobile device 1 is stationary, for example. In addition, the initial value g _{OFF, 0} [m] of the offset estimated value g _OFF [m] of the three-dimensional angular velocity sensor 90 is normally adjusted in the vicinity of “0”, so is set to “0”.

The initial value q _{OFF, 0} [m] of the estimated offset value q _OFF [m] of the three-dimensional magnetic sensor 70 is the magnetic data q ₀ output from the three-dimensional magnetic sensor 70 at time T = ₀ , the ground coordinate system Σ _G Using the vector ^G Bg [m] representing the geomagnetism Bg, the initial value μ ₀ [m] of the posture μ [m], and the matrix B (μ) shown in the equation (20) ( 21). The time T = vector ^G Bg representing the geomagnetism Bg from the ground coordinate system sigma _G at 0 [m] is the initial value _r 0 [m] of the geomagnetic intensity r [m], geomagnetic dip phi [m ] Is represented by the following formula (22) using the initial value φ ₀ [m].

In this way, the initial value generation unit 200 generates four different initial vectors INI [1] to INI [4] so that at least one of the values that are close to the true value is included. Then, the Kalman filter operation unit 300 operates in parallel the four Kalman filters KF [1] to KF [4] to which the four initial vectors INI [1] to INI [4] are respectively supplied.
In this case, the Kalman filter KF which operates by being supplied with the initial vector INI indicating a value close to the true value of the state vector at time T = 0 can accurately estimate the state of the system.

[3. Calculation using nonlinear Kalman filter]
Next, the calculation of the nonlinear Kalman filter performed by the Kalman filter KF will be described.

In general, the Kalman filter uses a state transition model that represents a change in the state of a dynamic system over time, 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 (23), and the observation model is given by the following equation (24). In the present embodiment, the state equation f appearing in the equation (23) and the observation equation h appearing in the equation (24) are nonlinear equations.

Hereinafter, the dimension of the state vector x _k is represented as “dim (x)”, and the dimension of the observation vector y _k is represented as “dim (y)”. In the present embodiment, dim (x) = 15 and dim (y) = 9. Further, the process noise w _k appearing in the equation (23) and the observation noise u _k appearing in the equation (24) are Gaussian noises centered on “0”.
Equation (23) 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.
Further, the expression (24) indicates 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 u 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 the observation noise _{u k} and _{U k.}

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 (25). As shown in the following equation (27), the Kalman filter uses the observed residual z _k , the estimated state vector x ⁻ _k , and the Kalman gain K _k shown in equation (26) 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 (28).
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 (24). . 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 (27), each element of the state vector x _k is updated by updating the estimated state vector x ⁻ _k to the updated state vector x ⁺ _k using the observation residual z _k. Can be brought closer to the true value.

The Kalman filter KF according to the present embodiment performs a nonlinear Kalman filter operation using a sigma point Kalman filter using unscented transformation.
FIG. 5 is a functional block diagram for explaining the operation of the Kalman filter KF [m]. As illustrated in FIG. 5, 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 [m] output from the initial value generation unit 200 to the state vector x ⁺ _k−1 .

Next, the sigma point generation unit 320 uses the covariance matrices P ⁺ _k−1 and Q _k−1 of “dim (x)” rows “dim (x)” columns to obtain “2 dim (x) +1” pieces. Generate sigma points for
Specifically, first, with respect to the average of a plurality of sigma points, a vector σ _k (1) shown in Expressions (29) and (30) is used by using a scaling parameter λ representing the spread of sigma points around the average. ) To σ _k (2dim (x)).

At this time, the sigma point generation unit 320 performs “2dim (x) +1” sigma expressed by Expression (31) and Expression (32) 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 (33), "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 (34), 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 (35), 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 (36), 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 calculate the estimated state vector x ⁻ _k by applying the state vector x ⁺ _k−1 to the state transition model. Functions as a state transition model unit 710.

On the other hand, the estimated observation value calculating unit 350, as shown in equation (37), 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 (38), 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.

Further, the estimated observation value vector calculation unit 360 calculates the covariance P ^yy _k of the observation residual as shown in the equation (39). 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 (40).

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 720 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 (25).
The Kalman gain assigning unit 380 calculates the mutual covariance matrix P ^xy _k as shown in Expression (41). Then, as shown in Expression (26), 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 (27) is executed.

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 (27) output from the Kalman gain assigning unit 380, as shown in the equation (27). Thus, the updated state vector x ⁺ _k is calculated. The adder 390, as shown in equation (28), the covariance _{P k} of the estimation error of the state vector _{x k,} P ^- updates from _k to ^P _{+ k.}

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 730 that calculates the updated state vector x ⁺ _k that updates ₁ .

  Next, of the state transition models used in the state transition model unit 710, 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 time T = k-1 attitude ^mu _{+ k-1} in 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 (42) 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.
The operator Ω on the right side of the equation (42) is defined by the equation (43). 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 (44). 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 (45).

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 the equation (34). 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 is a vector expressed in the direction and magnitude of the instrument coordinate system sigma _S of the internal magnetic field Bi to the portable device 1 of the component emitted. 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 change in the offset of the three-dimensional magnetic sensor 70, and the offset estimated value q of the three-dimensional magnetic sensor 70 at time T = k−1. It is difficult to formulate the estimated offset value q _{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 this embodiment, for convenience, the estimated offset value q _{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 q _{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 (46). 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 (27), 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.

  Next, among the observation models used in the observation model unit 720, 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 (47) using the angular velocity ω and the estimated offset 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 (48). Therefore, the estimated value γ _mag of the magnetic data q output from the three-dimensional magnetic sensor 70 is obtained by using the offset estimated value q _OFF of the magnetic sensor and the matrix B (μ) shown in the expression (20). 49).

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 (20) using the matrix B (μ) expressed by the equation (20) and the vector ^G G _RV. 50). Incidentally, the vector ^G G _RV is as shown in the following equation (51), a vector representing the gravitational acceleration in the ground coordinate system sigma _G, a three-dimensional vector obtained by normalizing the magnitude of the gravitational acceleration.

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

[4. Conclusion]
Each element of the initial vector INI, when a predetermined value set in advance, the posture initial value mu ₀ of mu, the initial value mu ₀ other elements of attitude mu (e.g., initial value r geomagnetic strength r ₀ and, in comparison with the initial value phi _0, etc.) of the geomagnetic dip phi, it is likely to be a large discrepancy value from the true value. For example, as described above, when the initial value μ ₀ is set to one predetermined fixed value, the initial value μ ₀ is shifted by 180 degrees from the true posture (true value) at the maximum. In this case, the state estimation device cannot correctly estimate the state of the system.
On the other hand, the state estimation device according to the present embodiment generates the initial values μ ₀ [1] to μ ₀ [4] of the posture μ using the image information Img and the position information Ps. _{μ 0 [1] ~μ 0 [} 4] it is possible to a value close to the true value of at least one initial value _mu 0 [Sel] of.
The Kalman filter KF [Sel] supplied with the initial value μ ₀ [Sel] close to the true value can accurately estimate the state of the system. That is, each element of the state vector x _k [Sel] output from the Kalman filter KF [Sel] has an accurate value close to the true value. The output information control unit 400, the Kalman filter KF [1] ~KF [4] the state vector _x k [1] that is outputted _~x k [4], and select the state vector _x k [Sel], this This is supplied to the output information generation unit 500. In this case, the output information generation unit 500 can generate output information that accurately represents the geomagnetism Bg, the posture of the mobile device 1, and the like.
Thus, the state estimation apparatus according to the present embodiment can accurately estimate the state of the system.

<B. Modification>
The present invention is not limited to the above-described embodiments, and for example, the following modifications are possible. Further, two or more of the modifications shown below can be appropriately combined within a range that does not contradict each other.

(1) Modification 1
In the embodiment described above, the object Obj is coordinates in ground coordinate system sigma _G was known landmarks, the present invention is not limited to such a mode, the object Obj is the sun, the moon, It may be a celestial body such as a star.
The state estimation device according to the modified example 1 is configured in the same manner as the state estimation device according to the above-described embodiment except that the initial value generation unit 200a is provided instead of the initial value generation unit 200. FIG. 6 is a functional block diagram of the initial value generation unit 200a. The initial value generation unit 200a is configured in the same manner as the initial value generation unit 200 except that an initial posture calculation unit 220a is provided instead of the initial posture calculation unit 220. The initial posture calculation unit 220a is configured in the same manner as the initial posture calculation unit 220 except that it includes a shooting target information acquisition unit 221a instead of the shooting target information acquisition unit 221.
The ROM included in the mobile device according to the first modification stores a look-up table storing information Pobj2 about each of a plurality of celestial bodies that can be candidates for the object Obj. Here, the specific content of the information Pobj2 is information on the orbit of each celestial body (for example, information on the position and time on the earth and the direction in which each celestial body can be seen at the position and time). The imaging target information acquisition unit 221a generates the second direction vector β based on the position information Ps of the mobile device, the information Obj2 of the object Obj, and the time information Time. Similar to the initial posture calculation unit 220, the initial posture calculation unit 220a can generate four initial values μ ₀ [1] to μ ₀ [4] of the posture μ in which at least one indicates a value close to the true value. . Therefore, the state estimation device according to the first modification can accurately estimate the state of the system.

(2) Modification 2
In the embodiment described above, the mobile device 1 includes the timepiece 130, but the mobile device according to the present invention is not limited to such a configuration, and the mobile device 1 may not include the timepiece 130. .

(3) Modification 3
The state estimation device according to the embodiment and the modification described above includes four Kalman filters KF [1] to KF [4] that operate based on the four initial vectors INI [1] to INI [4]. However, the present invention is not limited to such a form, and the predetermined number M of Kalman filters KF [1] operating in parallel based on the predetermined number M of initial vectors INI [1] to INI [M]. -KF [M] may be provided (M is a natural number satisfying 1 ≦ M).
In this case, the initial value generation unit 200 according to the modification 3 may be anything that generates a predetermined number M of initial vectors INI [1] to INI [M]. Specifically, the initial value generation unit 200 according to the modified example 3 calculates the posture A _C (posture A [1]) based on the first direction vector α and the second direction vector β, and generates a straight line AX (second by rotating the attitude _{a C} as an axis direction vectors beta) by a predetermined angle (2π / M), and calculates the attitude a [2] ~A [M] , the posture a [1] of the predetermined number M to a from [M], based on the initial orientation calculation unit 220 that generates the initial value _{μ 0 [1] ~μ} 0 posture mu [M], the initial value of a predetermined number _{M μ 0 [1] ~μ 0} [M] The initial vector generation unit 230 that generates the predetermined number M of initial vectors INI [1] to INI [M] may be used.
In this case, the output information control unit 400 only needs to select a Kalman filter with the highest estimation accuracy from a predetermined number M of Kalman filters KF [1] to KF [M].
When the predetermined number M is larger than “4”, the state estimation device according to the modified example 3 can estimate the state of the system more accurately and quickly than the state estimation device according to the above-described embodiment. It becomes possible. Further, when the predetermined number M is smaller than “4”, the state estimation device according to the modified example 3 can suppress the processing load in the calculation of the nonlinear Kalman filter to be smaller than that of the state estimation device according to the above-described embodiment. Therefore, it is possible to reduce the power consumption of the portable device in which the state estimation device is incorporated.

(4) Modification 4
The state estimation device according to the embodiment and the modification described above evaluates the estimation accuracy of the Kalman filter KF using the error evaluation value ^MAX z _k [m], but the present invention is limited to such an aspect. the invention is not, if the value that can evaluate the magnitude of the error between the true value of the values and the state vector indicating the state vector x _k, by evaluating the estimation accuracy of the Kalman filter KF using any value Also good. For example, using a covariance P _k of the estimation error of the state vector x _k, it may be configured to evaluate the estimation accuracy of the Kalman filter KF.

(5) Modification 5
The state estimation device according to the embodiment and the modification described above generates a plurality of initial vectors INI [1] to INI [M] based on the image information Img output when the camera 110 captures one object Obj. However, the plurality of Kalman filters KF [1] to KF [M] to which these are supplied are operated in parallel. However, the present invention is not limited to such an embodiment, and two cameras 110 are provided. One initial vector INI [1] is generated based on image information Img output when two objects Obj (object Obj1, object Obj2) are photographed, and the initial vector INI [1] is supplied to the Kalman filter KF [ 1] may be operated independently.
When the image information Img is generated by photographing the two objects Obj, based on the image information Img, the two first direction vectors α corresponding to the two objects Obj and the two objects Obj, respectively. Two corresponding second direction vectors β are generated. In this case, since the initial value μ ₀ (attitude A) of the attitude μ of the mobile device 1 can be uniquely specified based on the two first direction vectors α and the two second direction vectors β, true An accurate initial vector INI [1] having a value close to the value can be supplied to the Kalman filter KF [1].

FIG. 7 is a functional block diagram illustrating functions realized by the CPU 10 executing the state estimation program according to the modification 5 in the state estimation device according to the modification 5.
As shown in FIG. 7, the state estimation device according to the modified example 5 includes an initial value generation unit 200b instead of the initial value generation unit 200, a point including a Kalman filter calculation unit 300b instead of the Kalman filter calculation unit 300, and The configuration is the same as that of the state estimation device according to the embodiment except that the output information control unit 400 is not provided.
The initial value generation unit 200b includes an image processing unit 210b, an initial posture calculation unit 220b, and an initial vector generation unit 230b. The image processing unit 210b, the camera 110 has two objects Obj (object Obj1, object Obj2) based on image information Img output upon shooting, in the device coordinate system sigma _S, the object viewed from the portable device 1 Obj1 the first direction vector alpha ₁ representing the direction of, and calculates a first direction vector alpha ₂ representing the direction of the object Obj2 viewed from the portable device 1. The first direction vector α ₁ and the first direction vector α ₂ are vectors normalized in length to “1”.
The initial posture calculation unit 220b includes a shooting target information acquisition unit 221b and a posture calculation unit 222b. Photographing object information obtaining section 221b, on the basis of the information relating to two objects Obj camera 110 is photographed Pobj (or information Pobj2) and position information Ps, the ground coordinate system sigma _G, the object viewed from the portable device 1 Obj1 a second direction vector beta ₁ representing the direction of, and calculates a second direction vector beta ₂ which represents the direction of the object Obj2 viewed from the portable device 1. The second direction vector β ₁ and the second direction vector β ₂ are vectors whose lengths are normalized to “1”.
When the camera 110 captures two objects Obj and the posture of the mobile device 1 is A, Expressions (53) and (54) are established. Expressions (53) and (54) can be transformed into Expression (55). And Formula (53)-Formula (55) can be deform | transformed into Formula (56). In Expression (56), the matrix _Dα is a 3 × 3 matrix shown in Expression (57), and the matrix _Dβ is a 3 × 3 matrix shown in Expression (58). Therefore, the posture A can be uniquely calculated by the equation (59). The posture calculation unit 222b uses the two first direction vectors α ₁ and α ₂ and the two second direction vectors β ₁ and β ₂ , based on the equation (59), based on the equation (59). The initial value μ ₀ [1]) of the posture μ is calculated. The initial vector generation unit 230b calculates an initial vector INI [1] based on the initial value μ ₀ [1].
The Kalman filter operation unit 300b includes one Kalman filter KF [1], and an initial vector INI [1] is supplied to the Kalman filter KF [1]. The Kalman filter KF [1] periodically outputs a state vector x _k [1] (strictly, an updated state vector x ⁺ _k [1]). Then, the output information generation unit 500 generates output information based on the state vector x _k [1].
As described above, since the initial vector INI [1] is an accurate value close to the true value of the state vector x ₀ [1], the Kalman filter KF [1] can accurately and quickly estimate the state of the system. it can.

  Note that when the camera 110 captures one object Obj, the CPU 10 executes the state estimation program 100 according to the embodiment (or modified examples 1 to 4), and the camera 110 has two objects Obj (object Obj1, object Obj2). ) May be executed, the state estimation program according to Modification 5 may be executed.

(6) Modification 6
The state estimation device according to the above-described embodiment and the modification executes the operation of the nonlinear Kalman filter using the sigma point Kalman filter, but the present invention is not limited to such a form, and the extended Kalman filter For example, the calculation may be performed by appropriately applying a known nonlinear Kalman filter.
FIG. 8 is a functional block diagram showing functions realized when the state estimation program according to Modification 6 is executed. As illustrated in FIG. 8, the state estimation device according to the modification 6 is the same as the state estimation device according to the above-described embodiment and the modification, except that the Kalman filter KFa [m] is provided instead of the Kalman filter KF [m]. Configured. The Kalman filter KFa [m] is provided with a state transition model unit 710a instead of the state transition model unit 710 and a Kalman filter KF except for the point provided with the observation model unit 720a instead of the observation model unit 720. The configuration is the same as [m].
The state transition model unit 710a calculates the estimated state vector x ⁻ _k by applying the state vector x ⁺ _k−1 to the state transition model shown in Expression (23). The observation model unit 720a calculates the estimated observation value vector y ^- _k by applying the estimated state vector x ^- _k to the observation model shown in Expression (24).
As described above, in the calculation of the nonlinear Kalman filter according to the modified example 6, the state vector x _k is updated without generating a plurality of sigma points.

DESCRIPTION OF SYMBOLS 1 ... Portable apparatus, 10 ... CPU, 60 ... GPS part, 70 ... 3D magnetic sensor, 80 ... 3D acceleration sensor, 90 ... 3D angular velocity sensor, 100 ... State estimation program, 110 ... Camera, 200 ... Initial value generation , 210 ... Image processing unit, 220 ... Initial posture calculation unit, 221 ... Shooting target information acquisition unit, 222 ... Posture calculation unit, 230 ... Initial vector generation unit, 300 ... Kalman filter operation unit, 400 ... Output information control unit, 500 ... Output information generation unit, q ... Magnetic data, Img ... Image information, Ps ... Position information, Pobj ... Information, µ ... Attitude, µ ₀ ... Initial value of attitude µ, x ... State vector, y ... Observation value vector, z ... Observation residual, α ... First direction vector, β ... Second direction vector, Obj ... Object, Σ _S ... Equipment coordinate system, Σ _G ... Ground coordinate system.

Claims (5)

The system is incorporated in a device including a plurality of sensors that observe the system and output observation values, a position information output unit that outputs position information, and an image acquisition unit that images an object and outputs image information, A state estimation device for estimating the state of
One or more Kalman filters for updating the state vector using a state vector having a plurality of state variables as elements for estimating the state of the system and an observation value vector having each of the observation values as elements A Kalman filter operation unit;
An initial value generation unit that generates an initial vector that is an initial value of the state vector;
With
The plurality of state variables are:
Including a state variable that estimates the attitude of the device;
The initial value generator is
The posture of the device is estimated based on the image information output by the image acquisition unit when the image acquisition unit captures one or more objects and the position information output by the position information output unit. Calculating an initial posture estimated value that is an initial value of a state variable, and generating the initial vector having the initial posture estimated value as an element;
The state estimation apparatus characterized by the above-mentioned. The Kalman filter operation unit is
A predetermined number of the Kalman filters operating in parallel;
The initial value generator is
In the device coordinate system, which is a three-axis coordinate system fixed to the device, based on the image information, when the image acquisition unit captures one object and outputs the image information, from the device An image processing unit for calculating a first direction vector representing the direction of the one object seen;
Based on the position information, in a ground coordinate system that is a three-axis coordinate system fixed on the earth, a second direction vector representing the direction of the first object viewed from the device is calculated, and the first An initial posture candidate value is calculated based on the direction vector and the second direction vector, and the initial posture candidate value is rotated by a predetermined angle about the second direction vector as a rotation axis, whereby the predetermined number of the initial posture values are calculated. An initial posture calculation unit for calculating a posture estimation value;
An initial vector generation unit that generates the predetermined number of the initial vectors corresponding to each of the predetermined number of the initial posture estimation values, and supplies the initial vectors to the predetermined number of the Kalman filters, respectively.
The state estimation apparatus according to claim 1. The Kalman filter operation unit is
Comprising one Kalman filter,
The initial value generator is
In the device coordinate system, which is a three-axis coordinate system fixed to the device, based on the image information, when the image acquisition unit captures two objects and outputs the image information, from the device An image processing unit for calculating two first direction vectors representing the directions of the two viewed objects,
Based on the position information, in the ground coordinate system, which is a three-axis coordinate system fixed on the earth, two second direction vectors representing the directions of the two objects viewed from the device are calculated. An initial posture calculating unit that calculates the initial posture estimated value based on the two first direction vectors and the two second direction vectors;
An initial vector generation unit that generates the initial vector having the initial posture estimated value as an element and supplies the initial vector to the Kalman filter;
The state estimation apparatus according to claim 1. The state estimation device includes:
A time output unit for outputting time information;
The initial posture calculation unit
Calculating the second direction vector based on the position information and the time information;
The state estimation apparatus according to claim 2 or 3, wherein The object is
The coordinates in the ground coordinate system are known;
The initial posture calculating unit calculates the second direction vector based on the position information and coordinates of the object in the ground coordinate system;
The state estimation apparatus according to claim 2 or 3, wherein

Images