JP2013061309A - Kalman filter, state estimation device, method for controlling kalman filter, and control program of kalman filter - Google Patents

Abstract

PROBLEM TO BE SOLVED: To reduce an amount of calculation of a Kalman filter.SOLUTION: A Kalman filter KF includes: an estimation state vector calculation unit 140 that applies a state vector xwith an attitude qand a vector βdefined as elements to a state transition model and calculates a state vector xwith an attitude qand a vector βdefined as elements; and a covariance calculation unit 125 that calculates a covariance Pthat is an estimation error of the state vector x. The estimation state vector calculation unit sets a vector βto a value equal to the vector β. The covariance calculation unit 125 calculates, out of the covariance P, a component Pthat represents a covariance of the estimation error of the vector βas a sum of a component Pthat represents a covariance of an estimation error of the vector βout of the covariance Pof the estimation error of the state vector x, and a component Qthat represents a covariance of process noise of the vector βout of the covariance (Q) of the process noise of the state transition model.

Description

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

  2. Description of the Related Art In recent years, research and development of mobile devices having functions for estimating the direction of geomagnetism, the posture of mobile devices, and the like have been actively conducted with the improvement in performance and versatility of mobile devices. When estimating the direction of geomagnetism or the posture of a mobile device, the output from multiple sensors that measure different physical quantities is integrated, rather than using only the output from a single sensor such as a geomagnetic sensor. By estimating, an accurate value can be estimated at a higher speed.

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, a method using a nonlinear Kalman filter such as an extended Kalman filter (EKF) or a sigma point Kalman filter (SPKF) is known. .
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. Non-Patent Document 1 discloses a method for estimating an attitude by integrating output signals from a triaxial angular velocity sensor, a triaxial acceleration sensor, and a triaxial geomagnetic sensor using a sigma point Kalman filter using unscented transformation. Has been. The sigma point Kalman filter is a calculation method more suitable for modeling a nonlinear system than other Kalman filters. By using the sigma point Kalman filter, the state of the nonlinear dynamic system can be accurately estimated.

Japanese Patent Laid-Open No. 9-5104

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

However, since the computation by the sigma point Kalman filter is computationally intensive, a device on which the sigma point Kalman filter is mounted needs to include a large-capacity memory and a high-performance CPU. And when calculating by a sigma point Kalman filter, much electric power is consumed. In particular, when a device equipped with a sigma point Kalman filter is a portable device powered by a battery, there is a problem that it is difficult to use the device for a long time.
The present invention has been made in view of the above-described points, and an object of the present invention is to realize a state estimation device that simplifies the calculation of the sigma point Kalman filter and achieves high speed and low power consumption.

  The present invention will be described below. In addition, in order to make an understanding of this invention easy, although this embodiment, a modified example, and the reference sign of an accompanying drawing are attached in parentheses, this invention is not limited to this embodiment by it.

In order to solve the above-described problem, a Kalman filter according to the present invention includes a state vector (x ⁺ _k-1 ) having a first vector (β ⁺ _k-1 ) and a second vector (q ⁺ _k-1 ) as elements, Based on the state vector estimation error covariance (P ⁺ _k−1 ), the state transition model (f) is used to estimate the first vector (β ⁻ _k ) and the second estimated vector (q ⁻ _k). ) As an element, an estimated state vector calculation unit that calculates an estimated state vector (x ⁻ _k ), a covariance calculation unit that calculates a covariance (P ⁻ _k ) of an estimation error of the estimated state vector, and the state transition Using the model and the observation model (h), an estimated observation value vector calculation unit that calculates an estimated observation value vector (y ⁻ _k ) from the state vector, the estimated observation value vector, and outputs from a plurality of external sensors Observed Based on an observation value vector (y _k ) having elements as values, an observation residual generation unit that calculates an observation residual (e _k ), the observation residual, and the estimated state vector, An update unit that calculates a vector obtained by updating a state vector, the estimated state vector calculation unit sets the estimated first vector to a value equal to the first vector, and the covariance calculation unit includes the Of the covariance of the state vector estimation error, a component (P ⁺ _{ββ, k−1} ) representing the covariance of the estimation error of the first vector, and the process noise covariance generated in the state vector in the state transition model among the components representing the covariance of the process noise generated in the first vector _(Q ββ) by adding, among the covariance of the estimation error of the estimated state vector, the estimation error of the estimated first vector Components representing the covariance _(P ^- _{ββ, k)} is calculated, it is characterized.

According to this invention, the covariance calculation unit calculates a component representing the covariance of the estimation error of the estimated first vector among the covariances of the estimation error of the estimated state vector by adding the two matrices.
In general, the covariance of an estimation error of a vector is calculated by first calculating a vector representing the estimation error of the vector and then multiplying the vector representing the estimation error by a vector obtained by transposing the vector representing the estimation error. It is calculated | required by calculating. Therefore, in order to obtain the covariance of the estimation error of a certain vector, in addition to the calculation for calculating the vector representing the estimation error, the calculation corresponding to the square of the number representing the dimension of the certain vector is required.
The sigma point Kalman filter generates a plurality of vectors representing the estimation error of the estimated state vector based on the plurality of sigma points, and uses the generated vector representing the estimation error to calculate the covariance of the estimation error of the estimated state vector. calculate. Therefore, in order to obtain the covariance of the estimation error of the estimated state vector, it is necessary to repeat the calculation corresponding to the square of the number representing the dimension of the estimated state vector and the number of times corresponding to the number of sigma points. It becomes. As described above, the calculation for obtaining the covariance of the estimation error of the estimated state vector has a large amount of calculation and has been a factor of increasing the processing load of the sigma point Kalman filter.
On the other hand, the covariance calculation unit according to the present invention calculates a part of the covariance of the estimation state vector estimation error by a simple operation of adding two matrices. Compared to the case of calculation, the calculation amount related to the calculation for obtaining the covariance of the estimation error of the estimated state vector can be greatly reduced. As a result, the calculation of the sigma point Kalman filter can be speeded up and the power required for the calculation of the sigma point Kalman filter can be reduced.
Note that the estimated state vector calculation unit of the present invention sets the estimated first vector to a value equal to the first vector. However, the first vector does not change the value at all in the calculation of the Kalman filter. That is, the estimated state vector including the estimated first vector as an element is updated by the observation residual in the updating unit, and updated to a value (true value) that accurately represents the state of the system that is the estimation target of the Kalman filter. Thereby, the Kalman filter according to the present invention can accurately estimate the state of the system.

In the Kalman filter described above, the estimated state vector calculation unit generates a plurality of sigma points (χ ⁺ _k−1 ) based on the state vector and the covariance of the state vector estimation error. Using the generation unit, the state transition model, a state transition model unit that calculates a plurality of estimated sigma points (χ ⁻ _k ) from the plurality of sigma points, and the estimated state vector (x ⁻ _k ) An average calculation unit, wherein the state transition model unit includes, among the estimated sigma points, elements other than the elements used for calculating the estimated second vector (q ⁻ _k ) the first set equal to the generated elements on the basis of the vector (beta ^{+ _k-1),} the average calculating unit, the estimated first vector ( ^- a _k), is set to the first vector (β ⁺ _k-1) and equal, the estimated second vector (q ^- a _k), said plurality of putative sigma points (chi ^- based on _k) calculated It is preferable to do.

According to the present invention, the estimated value of the state vector is calculated using a plurality of sigma points. As a result, in the estimation of the state vector using the state transition model, the influence due to the error of the state vector and the influence due to the nonlinearity of the system targeted by the Kalman filter can be reduced, and the estimation accuracy in the state transition model can be reduced. Can be prevented. That is, according to the present invention, the Kalman filter can accurately estimate the state of the system.
Further, according to the present invention, the estimated first vector is set to a value equal to the first vector, and only the estimated second vector is calculated based on a plurality of estimated sigma points. It is possible to reduce the calculation load.

Next, the state estimation apparatus according to the present invention includes a plurality of sensors that observe the system and output observation values, respectively, and any one of the Kalman filters.
According to the present invention, it is possible to realize a state estimation device capable of performing a high-speed Kalman filter and having low power consumption.

  Next, a Kalman filter control method according to the present invention is a Kalman filter control method for estimating a system state, in which a state vector having a first vector and a second vector as an element and an estimation error of the state vector are shared. And a step of calculating an estimated state vector having the estimated first vector and the estimated second vector as elements using a state transition model, and a step of calculating a covariance of the estimated error of the estimated state vector And using the state transition model and the observation model, a step of calculating an estimated observation value vector from the state vector, the estimated observation value vector, and observation values output from a plurality of external sensors as elements. A step of calculating an observation residual based on the observation value vector, the state residual based on the observation residual, and the estimated state vector. Calculating an estimated state vector, and the step of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector, and estimates an estimation error of the estimated state vector. The step of calculating the covariance includes a component representing the covariance of the estimation error of the first vector among the covariance of the estimation error of the state vector, and a covariance of process noise generated in the state vector in the state transition model A component representing the covariance of the process noise occurring in the first vector is added, and the component representing the covariance of the estimation error of the estimated first vector is calculated from the covariance of the estimation error of the estimated state vector. It is characterized by calculating.

  According to the present invention, it is possible to significantly reduce the amount of calculation for calculating the covariance of the estimation error of the estimated state vector as compared with the case where the calculation is performed according to the conventional method. The power required for Kalman filter operation can be reduced.

  Next, a Kalman filter control program according to the present invention is a Kalman filter control program for estimating a system state, and a state vector having elements of a first vector and a second vector and a state vector estimation error are shared. And a process for calculating an estimated state vector having the estimated first vector and the estimated second vector as elements using a state transition model, and a process for calculating a covariance of the estimated error of the estimated state vector And using the state transition model and the observation model as an element, processing for calculating an estimated observation value vector from the state vector, the estimated observation value vector, and observation values output from a plurality of external sensors Based on the observation value vector, based on the processing for calculating the observation residual, the observation residual, and the estimated state vector A process of calculating a vector obtained by updating the state vector, and the process of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector, and the estimated state The process of calculating the covariance of the vector estimation error occurs in the state vector in the state transition model and the component representing the covariance of the first vector estimation error in the state vector estimation error covariance. Of the process noise covariance, a component representing the process noise covariance generated in the first vector is added, and the estimation error covariance of the estimated first vector out of the estimated error covariance of the estimated state vector is added. A component representing dispersion is calculated.

  According to the present invention, it is possible to significantly reduce the amount of calculation for calculating the covariance of the estimation error of the estimated state vector as compared with the case where the calculation is performed according to the conventional method. The power required for Kalman filter operation can be reduced.

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 of a Kalman filter concerning an embodiment of the present invention.

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

[1. Device configuration and software configuration]
FIG. 1 is a block diagram of a portable device according to an embodiment of the present invention, and FIG. 2 is a perspective view showing an appearance of the portable device. The mobile device 1 has a function of causing the orientation represented by the image to follow the orientation of the real space by rotating an image such as a map in accordance with the direction of the screen that changes by changing the orientation of the mobile device 1. Prepare. This function is realized by calculating the Kalman filter based on the outputs of various sensors and estimating the posture of the mobile device 1 and the like.

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

  The three-dimensional magnetic sensor 70 includes an X-axis magnetic sensor 71, a Y-axis magnetic sensor 72, and a Z-axis magnetic sensor 73. Each sensor can be configured using an MI element (magnetoimpedance element), an MR element (magnetoresistance effect element), or the like. The magnetic sensor I / F 74 AD-converts output signals from each sensor and outputs magnetic data m. The magnetic data m is vector data represented by three components of the x axis, the y axis, and the z axis.

By the way, 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. For this reason, the magnetic data m output from the three-dimensional magnetic sensor 70 includes a magnetic field generated by a component mounted on the device, in addition to a vector representing geomagnetism having a horizontal component toward the north of the magnetic pole and a vertical component in the dip direction. The value also includes the vector to be represented. Therefore, in order to accurately know the value of geomagnetism, it is necessary to perform a correction process for removing a vector representing the internal magnetic field generated by the component of the mobile device 1 from the vector data (magnetic data m) output from the three-dimensional magnetic sensor.
As described above, in order to obtain an accurate value of the geomagnetism that is a detection target, the value of the internal magnetic field to be removed from the data output from the three-dimensional magnetic sensor 70 in the correction process is set to the offset m _OFF of the three-dimensional magnetic sensor 70. Call it.
The geomagnetism detection unit 70 also detects a magnetic field generated by an external object outside the mobile device 1. However, in this embodiment, the magnetic field generated by the external object is assumed to be negligibly small.

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

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

  The CPU 10 executes the state estimation program 100 stored in the ROM 30 to thereby execute a plurality of physical quantities representing the state of the dynamic system, for example, the posture of the mobile device 1, the offset of the three-dimensional magnetic sensor 70, the strength of geomagnetism, Estimate physical quantities such as dip angles. That is, the portable device 1 functions as a state estimation device when the CPU 10 executes the state estimation program 100.

  The state estimation program 100 includes an initial value generation module 110 and a Kalman filter module 120. The Kalman filter module 120 executes the calculation of the Kalman filter KF.

[2. Observation target and estimation target of Kalman filter]
In general, the Kalman filter estimates a state transition model that estimates changes over time of a plurality of physical quantities representing the state of a dynamic system and values (observed values) measured by a plurality of sensors that observe the dynamic system. An observation model. Then, the Kalman filter uses a state transition model to estimate a state vector after a unit time has elapsed from a state vector whose elements are a plurality of physical quantities (state variables) representing the state of the dynamic system at a certain time. Next, based on the estimated state vector, the Kalman filter estimates the value of an observation value vector whose elements are output values of a plurality of sensors that measure the state of the dynamic system. Furthermore, based on the observation residual calculated as the difference between the estimated observation vector and the observation vector whose element is the actual sensor output value, the estimated state vector is converted to the actual value (true value). To a value close to) and output the updated state vector.
The Kalman filter repeats the above operation to bring each of a plurality of state variables constituting the state vector close to a value (true value) that accurately represents the actual physical quantity.

In this embodiment, the Kalman filter KF includes the posture q of the mobile device 1, the geomagnetic strength r, the geomagnetic dip angle θ, the angular velocity ω of the mobile device 1, the estimated value g _O of the offset g _OFF of the three-dimensional angular velocity sensor 90, and An estimated value m _O of the offset m _OFF of the three-dimensional magnetic sensor 70 is adopted as a state variable, and these are used as estimation targets.
A state vector x _{k at} time T = k having these state variables as elements 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 present embodiment, the posture q is expressed using quaternions. A quaternion is a four-dimensional number that represents the posture (rotation state) of an object. Specifically, a coordinate system fixed on the ground (for example, an arbitrary point on the ground as an origin and three directions orthogonal to each other, for example, horizontal east, horizontal north, and vertical upward, respectively, are x-axis and y-axis. , And the z coordinate system), and defines a posture q in which each axis of the coordinate system fixed on the ground and each axis of the coordinate system fixed on the mobile device 1 coincide with each other as a reference posture, The reference posture is expressed as q = (0, 0, 0, 1). At this time, the arbitrary posture q of the mobile device 1 can be expressed as a posture rotated by an angle ψ with the direction of the unit vector ρ as the rotation axis with respect to the reference posture. The posture q after rotation is given by equation (2).

The observation target of the Kalman filter in this embodiment is magnetic data m output from the three-dimensional magnetic sensor 70, acceleration data a output from the three-dimensional acceleration sensor 80, and angular velocity data g output from the three-dimensional angular velocity sensor 90. is there. The observed value vector has these magnetic data m, acceleration data a, and angular velocity data g as elements. Observation value vector y _{k at} time T = k is given by equation (3).

The initial value generation module 110 calculates an initial value INI having the state variable at time T = 0 as an element. The initial value INI, the initial value q ₀ of the attitude _q, the initial value r ₀ of the geomagnetic intensity _r, the initial value theta ₀ geomagnetic dip _theta, offset estimation of the initial value omega _0, 3-dimensional angular velocity sensor 90 of the angular velocity omega including the initial value m _{O, 0} offset estimate _{m O} value _{g O} initial value g _{O, 0,} and three-dimensional magnetic sensor 70 as an element.
The initial value INI may be appropriately set to a value such that the state variable converges to an accurate value as soon as possible by calculation of the Kalman filter, but may be set to the following value, for example.

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

The initial value ω ₀ of the angular velocity ω is set to “0”, for example, assuming that the mobile device 1 is stationary. Since the initial value g _{O, 0} of the offset estimated value g _O of the three-dimensional angular velocity sensor 90 is normally adjusted in the vicinity of “0”, it is set to “0”. For the initial value q ₀ of the posture q, for example, assuming that the mobile device 1 is stationary in a certain direction, a value is set to reduce the deviation from the actual initial posture.

The initial value m _{O, 0} of the offset estimated value m _O of the three-dimensional magnetic sensor 70 is, for example, the observed value m ₀ of the three-dimensional magnetic sensor 70 at time T = 0, the initial value q ₀ of the posture q, and the portable device 1 Using the geomagnetic vector m _{typical of the} area to be used and the matrix B (q) shown in Equation (5), the value is set to the value given by Equation (4) below. Here, the matrix B (q) is a coordinate for expressing a vector expressed in the coordinate system fixed on the ground in the coordinate system fixed to the mobile device 1 when the mobile device 1 is in the posture q. It is a transformation matrix. The ROM 30 stores a lookup table LUT2 that stores positional information and the geomagnetic vector m _typical in association with each other. The initial value generation module 110 obtains the geomagnetic vector m _typical by referring to the lookup table LUT2 based on the position information generated by the GPS unit 60, calculates the offset estimated value m _O by calculating equation (4). The initial value m _{O, 0 of} is obtained.

The initial value generation module 110, the initial value q ₀ of the attitude q which is generated as described _above, the initial value r ₀ of the geomagnetic intensity _r, the initial value theta ₀ geomagnetic dip _theta, an initial value of the angular velocity omega omega ₀ , an initial value INI which is a vector having the initial value g _{O, 0} of the offset estimated value g _O of the three-dimensional angular velocity sensor 90 and the initial value m _{O, 0} of the offset estimated value m _O of the three-dimensional magnetic sensor 70 as elements. Is generated and output.

[3. Calculation using Kalman filter]
Next, an outline of calculation by the Kalman filter KF according to the present embodiment will be described.
The Kalman filter KF uses a state transition model that represents a change in the dynamic system state over time, and a unit time has elapsed from a state vector x _k-1 indicating the system state at a certain time (time T = k-1). and it estimates the state vector x _k after (time T = k) was, this estimate, the estimated state vector (estimated state vector) x ^- output as _k. The Kalman filter KF is based on the state vector x ⁻ _k estimated by the state transition model using an observation model representing the relationship between the observation value vector y _k and the state vector x _k whose elements are outputs from various sensors. The observed value vector y _k is estimated, and this estimated value is output as an estimated observed value vector (estimated observed value vector) y ⁻ _k . Note that the Kalman filter KF of the present embodiment is configured by a sigma point Kalman filter that is a nonlinear Kalman filter. The sigma point Kalman filter sets a plurality of sigma points χ _k−1 around the state vector x _k−1 , and applies these sigma points χ _k−1 to the state transition model, so that a unit time elapses. This is an operation for estimating the subsequent sigma point χ ⁻ _k and calculating the average of the estimated value (estimated sigma point) χ ⁻ _k of the sigma point to obtain the estimated state vector x ⁻ _k . Therefore, strictly speaking, the estimated observation value vector y ⁻ _k is calculated based on the estimated values χ ⁻ _k of a plurality of sigma points existing around the estimated state vector x ⁻ _k .
Then, the Kalman filter KF is estimated observed value vector y ^- calculates and _k, a difference between the observed value vector y _k to the actual observed value element as observed residuals e _k. The Kalman filter KF calculates the Kalman gain K _k based on the estimated state vector x ⁻ _k and the estimated observation value vector y ⁻ _k . Then, the Kalman filter KF updates the state vector x ⁻ _k estimated using the observation residual e _k and the Kalman gain K _k , and calculates the updated state vector (the vector obtained by updating the state vector) x ⁺ _k . To do.
By calculating the Kalman filter KF that repeatedly updates the state vector x _k as described above, the state vector x _k can be brought close to an accurate value close to a value (true value) that accurately represents the actual physical quantity.

[3.1. Overview of Kalman filter operation section]
The state transition model of the Kalman filter KF in this embodiment is given by the following equation (6), and the observation model is given by the following equation (7). In the present embodiment, the function f appearing in Expression (6) and the function h appearing in Expression (7) are nonlinear functions.

Here, the state vector x _k is an n-dimensional vector, and the observation value vector y _k is an m-dimensional vector. In this embodiment, n = 15 and m = 9. Further, the process noise u _k appearing in the equation (6) and the observation noise v _k appearing in the equation (7) are Gaussian noises centered on zero.
Equation (6) shows that the state vector x _k at time T = k is obtained by substituting the state vector x _k−1 at time T = _k−1 for the function f, and at time T = k−1. It shows that the noise is estimated by adding the process noise u _k−1 .
Equation (7) shows that the observed value vector y _k at time T = k is obtained by substituting the state vector x _k at time T = _k into the function h, and the observed noise v _{k at} time T = k. It shows that it estimates by adding.
The covariance of the process noise u _k is represented as Q, and the covariance of the observation noise v _k is represented as R.

The observation residual e k at time T = k is a vector determined based on the actual observation vector y k and the estimated observation vector y − k, and is expressed by the following equation (8). The The Kalman filter KF uses the observation residual e k and the Kalman gain K k shown in Equation (9) to update and update the estimated state vector x − k as shown in Equation (10) below. The later state vector x + k is calculated. Further, the Kalman filter KF updates the estimation error covariance P k of the state vector x k as shown in the following equation (11).
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 .
Also, P yy k is the covariance of the observation residuals e k, P xy k is the state vector x is estimated - is the cross-covariance matrices between the k - and k, observed value vector y estimated .

The observation value vector y ⁻ _k estimated using the observation model is a theoretical value calculated based on the state vector x ⁻ _k estimated using the observation model, as shown in Equation (7). Thus, the observed value vector y was estimated using the observation model ^- observation residuals e _k is the difference between the observed value vector y _k based on actual sensor output and _k is estimated state vector x ^- actually a _k This is a value indicating the degree of approximation with a value (true value) that accurately represents the physical quantity.
Kalman filter KF, as shown in equation (10), with the observed residual e _k, estimated state vector x ^- a _k, is updated to the state vector x ⁺ _k after update close to the true value.

[3.2. Calculation with Sigma Point Kalman Filter]
In the present embodiment, the Kalman filter KF is configured by a sigma point Kalman filter using unscented transformation. Hereinafter, the details of the calculation by the Kalman filter KF will be described with reference to the functional block diagram of the Kalman filter KF shown in FIG.

The delay unit 121 delays the updated state vector x ⁺ _k output from the adder 132 by a unit time (a time corresponding to time T = k from time T = k−1), so that the state vector x ⁺ _k is delayed. ⁺ _K−1 is generated and output to the sigma point generator 122. In the first calculation (time T = 0), the delay unit 121 generates the state vector x ⁺ _k−1 using the initial value INI output from the initial value generation module 110.
Further, the delay unit 121 delays the estimated error covariance P ⁺ _k of the updated state vector x ⁺ _k output from the subtracter 133 by unit time, thereby reducing the estimated error of the state vector x ⁺ _k−1 . A covariance P ⁺ _k−1 is generated and output to the sigma point generation unit 122 and the covariance calculation unit 125.

The sigma point generator 122 generates a sigma points χ ⁺ _k−1 (i) (i = 1, 2,... A) based on the state vector x ⁺ _k−1 (a is 2 or more). Natural number). The generation of the sigma point χ ⁺ _k−1 (i) applies a known method shown below.
Sigma point generation unit 122, first, from the covariance ^P _{+ k-1} of the estimation error of the state vector ^x _{+ k-1,} n rows and n columns representing the square root of the covariance ^P _{+ k-1} shown in formula (12) Generate a matrix of Next, the sigma point generation unit 122 determines a sigma point calculation vectors based on a matrix representing the square root of the covariance P ⁺ _k−1 and various coefficients. The sigma point calculation vector is an n-dimensional vector. Thereafter, the sigma point generation unit 122 generates a sigma points χ ⁺ _k−1 (i) based on the state vector x ⁺ _k−1 and the a sigma point calculation vectors, Output to the state transition model unit 123.
Note that various coefficients used in generating a sigma points χ ⁺ _k−1 (i) are values used for generating weights w _i in equation (14) described later, and are determined by a known method. . Also, the zero sigma point calculation vectors may include zero vectors.

The state transition model unit 123 applies each of the a sigma points χ ⁺ _k−1 (i) at time T = k−1 to the state transition model as shown in Expression (13), so that time T = the estimated value of a number of sigma points in k chi ^- calculating the _k (i).

Then, the average calculation unit 124, as shown in equation (14), time T = estimate of a number of sigma points in k chi ^- weighted average to calculate the _{k (i),} was estimated A state vector x ^- _k is calculated and output. The weight w _i appearing in the equation (14) is determined based on various coefficients used in the generation of the sigma point χ ⁺ _k−1 (i).
As described above, the sigma point generation unit 122, the state transition model unit 123, and the average calculation unit 124 serve as the estimated state vector calculation unit 140 that calculates the estimated state vector x ⁻ _k from the state vector x ⁺ _k−1. Function.

Covariance calculation unit 125, the state vector being estimated x ^- calculates the _k ^- covariance P of the estimation error of _k. Covariance P of _k of the estimation error ^{- -} estimated state vector x _k as shown in equation (15), an estimate of a number of sigma points chi ^- _{k (i),} the estimated state vector x ^- It can be calculated based on the covariance Q of _k , weight w _i , and process noise u _k−1 .
However, in the present embodiment, from the viewpoint of reducing the processing load, some components of the covariance P ⁻ _k are calculated based on Expression (30) described later without using Expression (15). Details of this point will be described later.

On the other hand, the observation model unit 126, as shown in equation (16), time T = estimate of a number of sigma points in k chi ^- each of _{k (i),} by applying to the observation model, a number The estimated observation value γ _k (i) of is calculated.

And the average process part 127 calculates the estimated observation value vector y ^- _k by calculating the average of the a estimated observation value (gamma) _k (i), as shown in Formula (17).
As described above, the sigma point generation unit 122, the state transition model unit 123, the observation model unit 126, and the average processing unit 127 perform estimation to calculate the estimated observation value vector y ⁻ _k from the state vector x ⁺ _k−1. It functions as the observation value vector calculation unit 150.

Next, the average processing unit 127 calculates the covariance P ^yy _k of the observation residual shown in Expression (18).

Subtractor (observed residual generating unit) 131, as shown in Equation (8), the observed value vector y _k, the estimated observed value vector y ^- calculated as the difference between _k, the observation residuals e _k To do.
The Kalman gain assigning unit 128 calculates a mutual covariance matrix P ^xy _k shown in Expression (19). Next, as shown in Equation (9), the Kalman gain assigning unit 128 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. . Thereafter, the Kalman gain assigning unit 128 calculates the second term (K _k e _k ) on the right side of Expression (10), and outputs the result to the adder 132.
Further, the Kalman gain assigning unit 128 calculates the second term (K _k P ^yy _k K ^T _k ) on the right side of Equation (11) based on the covariance P ^yy _k of the observation residual and the Kalman gain K _k. The result is output to the subtracter 133.

As shown in Expression (10), the adder (update unit) 132 outputs the estimated state vector x ⁻ _k and the second term (K _k ) on the right side of Expression (10) output from the Kalman gain assigning unit 128. e _k ) and the updated state vector x ⁺ _k is calculated.
Subtractor 133, as shown in Equation (11), the state vector being estimated x ^- a right-hand side of the _k, equation output from the Kalman gain applying unit 128 (11) ^- covariance P of the estimation error of the _k as the difference between the term _{^{2 (K k P yy k K}} T k), calculates a covariance ^P _{+ k} of the estimation error of the state vector ^x _{+ k} after update.

[3.3. State transition model]
Next, the state transition model used in the calculation of the state transition model unit 123 will be described.

In the state transition model according to the present embodiment, among the state variables constituting the state vector x _k , the state transition for the posture q is expressed by using the function f _q (q, ω) shown in the expression (21) using the expression ( 20). Equation (20), the time T = state vector is estimated in the k x ^- _k is a state variable which constitute the attitude q ^- _k constitute the state vector ^x _{+ k-1} at time T = k-1 state It shows that the estimation is based on the posture q ⁺ _k−1 and the angular velocity ω ⁺ _k−1 which are variables.
The function f _q (q, ω) shown in Expression (21) is a function that represents only the part for calculating the posture q in the function f shown in Expression (6). The operator Ω on the right side of Equation (21) is defined by Equation (22). Here, I _{3 × 3} represents a unit matrix of 3 rows and 3 columns, and for the three-dimensional vector l = (l ₁ , l ₂ , l ₃ ), the symbol [l ×] is defined by Expression (23). To do. Further, when the measurement time interval (interval from time T = k−1 to time T = k) is represented by Δt, the component Ψ constituting the operator Ω is defined by Expression (24).

Since the posture q is expressed by a quaternion, the normalization condition || q || = 1 needs to be satisfied. However, when the state vector x _k is updated using the sigma point Kalman filter, the updated state vector x ⁺ _k is weighted to the estimated value χ ⁻ _k (i) of the sigma point, as shown in Expression (14). _Since it is calculated as a weighted average with _i added, the norm of the state vector x _k−1 (before update) and the norm of the state vector x ⁺ _k after update may be different values. Therefore, when performing the calculation of sigma point Kalman filter, and the norm of the (pre-update) the state vector x _k-1 is the element position q _k-1, which is an element of the state vector x ⁺ _k after update attitude q _k There is a possibility of different values from the norm of. That is, when performing the sigma point Kalman filter calculation, there is a possibility that the normalization condition for the posture q is not satisfied.
Therefore, when any calculation is performed on the posture q, the result after the calculation is normalized with the size of the vector itself. In order to maintain the normalization condition more strictly, the posture q _k among elements constituting the state vector x _k is limited to only 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 of the present embodiment, for convenience, time T = and strength r _k and dip theta _k geomagnetic at k, the time T = geomagnetic strength in k-1 r _k-1 and dip theta _k The assumption is made that ₋₁ is an equal value.
Similarly, it is difficult to predict a change in the estimated offset value g _O 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 _{O, K} at time T = k and the estimated offset value g _{O, K−1 at} time T = k−1 are equal values. Put the premise that.

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 m _OFF of the three-dimensional magnetic sensor 70 is an internal magnetic field generated by components of the mobile device 1. Therefore, the offset m _OFF 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 m _OFF 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 amount of change in timing and offset _{m OFF} the offset _{m OFF} changes, offset estimate m _O at time T = _k-1, using the _K-1, at time T = k It is difficult to formulate the estimated offset value m _{O, k} .
Therefore, in the state transition model according to the present embodiment, for the sake of convenience, it is assumed that the estimated offset value m _{O, k} at time T = k is equal to the estimated offset value m _{O, k−1 at} time T = k−1. .

As described above, in the state transition model according to the present embodiment, as shown in the following equation (25), it is assumed that the state variables other than the posture q among the state variables constituting the state vector x _k do not change from the previous time. Put.

Here, when the state vector x _k shown in Expression (1) is expressed separately as shown in Expression (26) into a posture q _k and a vector β _k representing an element other than the posture q _k , the vector For β _k , equation (27) holds. That is, of the state vector ^x _{+ k-1,} the posture and the vector (first vector) beta ⁺ _k-1 representing a (second ^vector) _{q + k-1} other elements, the estimated state vector x ^- _k- Of these, a vector (estimated first vector) β ^- _k representing an element other than the posture (estimated second vector) q ^- _k is equal.

When Expression (27) holds, as is clear from Expression (13), the element corresponding to the vector β ⁺ _k−1 in the sigma point χ ⁺ _k−1 (i) and the estimated value χ ^{− of the} sigma point vector of _k (i) _β ^- the corresponding element _k, equal.
Therefore, when Expression (27) holds, the component representing the covariance of the estimation error of the vector β ⁺ _k−1 out of the covariance P ⁺ _k−1 of the estimation error of the state vector x ⁺ _k−1 and the estimation state vector x ^- estimation error covariance P of _k ^- among _k, the vector beta ^- the component representing the covariance of the estimation error of _k, to be considered the covariance Q of the process noise u _k-1 Can be considered equal (see paragraph [0046]).

Here, as shown in Expression (28), the covariance P _k is divided into four matrices: a matrix P _{qq, k} , a matrix P _{qβ, k} , a matrix P _{qβ, k} ^T , and a matrix P _{ββ, k.} To do. The matrix P _{qq, k} is a 4 _× 4 matrix determined based on the posture q _k . The matrix P _{qβ, k} is a 4 _× 11 matrix determined based on the posture q _k and the vector β _k . The matrix P _{ββ, k} is an 11 _× 11 matrix determined based on the vector _βk . Specifically, the matrix P _{ββ, k} is a matrix representing the covariance of the estimation error of the vector _βk .

Further, the process of the covariance Q of the noise _{u k,} as in Equation (29), with four rows and four columns of the matrix _{Q qq,} 4 rows 11 columns of matrix _{Q Q [beta],} and the matrix Q _Betabeta of 11 rows 11 columns Represent.
As shown in equation (6), the process noise u _k is the process noise that occurs when applying the state vector x _k to the state transition model, the covariance Q shown in equation (29), the process noise u _k Represents the covariance of. Therefore, the matrix Q _ββ appearing in the equation (29) is a vector β representing an element other than the posture q _k in the state vector x _k among the process noise generated when the state vector x _k is applied to the state transition model. represents the covariance of the process noise occurring in _k .
In this case, the matrix P ^- _{ββ, k,} as shown in equation (30), the matrix ^{P +} _ββ, and _k-1, is defined as the sum of the matrix _Q ββ.

When all the components of the covariance P ⁻ _k are _obtained based on the equation (15), an operation for obtaining an n _× n matrix by multiplying an n _× 1 vector and a 1 _× n vector is performed by a It will be necessary to repeat.
On the other hand, when Expression (30) is used, the matrix P ⁻ _{ββ, k} is defined as the matrix P ⁺ _{ββ, k−1} that has been calculated one step before (at time T = k−1), the matrix Q _ββ , Are simply added. Then, the covariance P ^- _k is a matrix _P ^- _ββ, components other than _k, namely, four rows and four columns of the matrix ^P _{- qq, k} and 4 rows 11 columns of the matrix P ^- _{Q [beta], k} only, formula (15 ). Consequently, the covariance P of 15 rows 15 columns ^- all components of _k, as compared with the case of calculating using the equation (15), can be greatly reduced calculation amount in the covariance calculation unit 125 and Become.

Note that, in the state transition model, as shown in Expression (25) and Expression (27), the state vector x _k is the Kalman filter even when it is assumed that there is no change from the previous time except for the posture q. It is updated based in the operation of the observation residuals e _k. Accordingly, the vector β _k representing elements other than the posture q _{k is} also updated so as to approach the true value based on the observation residual e _k while repeating the Kalman filter calculation.

[3.4. Observation model]
Next, the observation model used in the calculation performed by the observation model unit 126 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 (31) using the angular velocity ω and the estimated offset value g _O of the three-dimensional angular velocity sensor 90.

In the coordinate system fixed on the ground, a vector representing geomagnetism is given by Expression (32). Therefore, the estimated value γ _mag of the magnetic data m output from the three-dimensional magnetic sensor 70 is obtained by using the estimated offset value m _O of the three-dimensional magnetic sensor 70 and the matrix B (q) expressed by Equation (5). Is given by equation (33).

Further, in a coordinate system fixed on the ground, a vector obtained by normalizing a vector representing gravity with gravitational acceleration is given by Expression (34). Therefore, the estimated value γ _acc of the acceleration data a output from the three-dimensional acceleration sensor 80 is given by Expression (35) using the matrix B (q) shown by Expression (5).

Therefore, substituting Equation (3) into the first term on the right side of Equation (8) and expressing the second term on the right side of Equation (8) using Equation (31), Equation (33), and Equation (35). Thus, the equation (8) can be transformed into the following equation (36). At this time, the observation residual _ek is calculated by Expression (36).

[4. Conclusion]
As described above, the state estimation device according to the present embodiment represents the state vector x _k used in the calculation of the sigma point Kalman filter by separating it into the posture q _k and the vector β _k, and the state vector x ^{+ _k-1} which is a part of the elements constituting the vector β ^{+ _k-1,} the state vector x is estimated ^- vector beta is a part of the elements constituting the _k ^- and _k is put premise that equal .
The state estimation apparatus according to the present embodiment, the estimated state vector x ^- estimation error covariance P of _k ^- vector of _k beta ^- a component representing the covariance of the estimation error of _k, the state vector x ⁺ _The component representing the covariance of the estimation error of the vector β ⁺ _k−1 in the covariance P ⁺ _k−1 of the estimation error of _{k−1 and} the vector β ⁺ _k− of the covariance Q of the process noise u _k−1. It was calculated by simply adding the component representing the covariance of the process noise occurring in ₁ .
Thus, the covariance P ^- all components of _k as compared with the case of calculating based on equation (15), it is possible to greatly reduce the computational load of the sigma points Kalman filter, the processing speed of the state estimation device Improvement and reduction in power consumption of the portable device 1 are possible.

<B. Modification>
The present invention is not limited to the above-described embodiments, and for example, the following modifications are possible. Also, two or more of the modifications shown below can be combined.

(1) Modification 1
In the above-described embodiment, the elements constituting the state vector x include the posture q of the mobile device 1, the geomagnetic strength r, the geomagnetic dip angle θ, the angular velocity ω of the mobile device 1, and the estimated offset value of the three-dimensional angular velocity sensor 90. Although g _O and the estimated offset value m _O of the three-dimensional magnetic sensor 70 are employed, and these six state variables are the targets of estimation in the calculation of the Kalman filter, the present invention is not limited to this. For example, the state vector x may be constituted by a part of the six state variables, and the part of the state variables may be an estimation target in the Kalman filter calculation.

(2) Modification 2
In the above-described embodiment, the observed value vector y is converted from the magnetic data m output from the three-dimensional magnetic sensor 70, the acceleration data a output from the three-dimensional acceleration sensor 80, and the angular velocity data output from the three-dimensional angular velocity sensor 90. Although the vector having g as an element is used, the present invention is not limited to this, and the observed value vector y is generated using only part of the magnetic data m, acceleration data a, and angular velocity data g. May be.

(3) Modification 3
In the embodiment described above, the average calculation unit 124, as shown in equation (14), the state vector x is estimated ^- the _k, the estimated value of a number of sigma points chi ^- as a weighted average of the _{k (i)} Although calculated, the present invention is not limited to this.
For example, the average calculation unit 124 simply sets the state vector x ⁺ _k−1 as the vector β ⁻ _k that is a part of the elements constituting the estimated state vector x ⁻ _k without using Expression (14). You may set to the value equal to vector (beta) ⁺ _k-1 which is a part of component. In this case, the average calculation unit 124, the estimated state vector x ^- among _k, the attitude q ^- for _k only, is calculated by a calculation for calculating a weighted average of equation (14). As a result, among the estimated state vectors x ^- _k which are 15-dimensional vectors, the 11-dimensional vector β ^- _k can be calculated by simple value substitution. The processing load related to the calculation of x ⁻ _k can be greatly reduced.

DESCRIPTION OF SYMBOLS 1 ... Portable apparatus, KF ... Kalman filter, 121 ... Delay part, 122 ... Sigma point generation part, 123 ... State transition model part, 124 ... Average calculation part, 125 ... Covariance calculation part, 126 ... Observation model part, 127 ... Average Processing unit 127 ... Estimated observation value vector calculation unit, 128 ... Kalman gain assignment unit, 131 ... Subtractor, 132 ... Adder, 133 ... Subtractor, 140 ... Estimated state vector calculation unit, 150 ... Estimated observation value vector calculation unit , K ... Kalman gain, x k _... state vector, x - ^k _... estimated state vector, q k _... posture, beta _{k ...} vector, covariance of the estimation error of P k _... state vector, Q ... process noise co Variance, e _k ... observation residual, y _k ... observation value vector, y ⁻ _k ... estimated observation value vector.

Claims (5)

Based on the state vector having the first vector and the second vector as elements and the covariance of the estimation error of the state vector, using the state transition model, the estimated first vector and the estimated second vector are elements. An estimated state vector calculation unit for calculating an estimated state vector;
A covariance calculation unit for calculating a covariance of an estimation error of the estimated state vector;
An estimated observation value vector calculation unit for calculating an estimated observation value vector from the state vector using the state transition model and the observation model;
An observation residual generation unit that calculates an observation residual based on the estimated observation vector and an observation vector that includes observation values output from a plurality of external sensors;
An update unit that calculates a vector obtained by updating the state vector based on the observation residual and the estimated state vector;
The estimated state vector calculation unit sets the estimated first vector to a value equal to the first vector,
The covariance calculation unit includes a component representing a covariance of the estimation error of the first vector among covariances of the estimation errors of the state vector, and a covariance of process noise generated in the state vector in the state transition model. Of these, the component representing the covariance of the process noise generated in the first vector is added to calculate the component representing the covariance of the estimated error of the estimated first vector among the estimated error covariance of the estimated state vector. A Kalman filter characterized by that. The estimated state vector calculation unit includes:
A sigma point generator that generates a plurality of sigma points based on the state vector and the covariance of the estimation error of the state vector;
A state transition model unit that calculates a plurality of estimated sigma points from the plurality of sigma points using the state transition model;
An average calculation unit for calculating the estimated state vector,
The state transition model part is:
Of the estimated sigma points, elements other than the elements used for calculating the estimated second vector are set to values equal to the elements generated based on the first vector of the sigma points,
The average calculator is
The estimated first vector is set to a value equal to the first vector, and the estimated second vector is calculated based on the plurality of estimated sigma points.
The Kalman filter according to claim 1.   A state estimation apparatus comprising: a plurality of sensors that observe the system and output observation values, respectively; and the Kalman filter according to claim 1. A Kalman filter control method for estimating a system state,
Based on the state vector having the first vector and the second vector as elements and the covariance of the estimation error of the state vector, using the state transition model, the estimated first vector and the estimated second vector are elements. Calculating an estimated state vector;
Calculating an estimated error covariance of the estimated state vector;
Calculating an estimated observation value vector from the state vector using the state transition model and the observation model;
A step of calculating an observation residual based on the estimated observation vector and an observation vector having elements of observation values output from a plurality of external sensors;
Calculating a vector obtained by updating the state vector based on the observation residual and the estimated state vector, and
The step of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector;
The step of calculating a covariance of the estimation error of the estimated state vector includes a component representing a covariance of the estimation error of the first vector among the covariance of the estimation error of the state vector, and the state in the state transition model. Among the process noise covariances generated in the vector, a component representing the process noise covariance generated in the first vector is added to estimate the estimated first vector out of the covariances of the estimated state vector estimation errors. A method of controlling a Kalman filter, characterized in that a component representing an error covariance is calculated. A Kalman filter control program that estimates the state of a system,
Based on the state vector having the first vector and the second vector as elements and the covariance of the estimation error of the state vector, using the state transition model, the estimated first vector and the estimated second vector are elements. A process of calculating an estimated state vector;
Processing for calculating a covariance of an estimation error of the estimated state vector;
A process of calculating an estimated observation value vector from the state vector using the state transition model and the observation model;
A process for calculating an observation residual based on the estimated observation vector and an observation vector having elements of observation values output from a plurality of external sensors;
Based on the observation residual and the estimated state vector, a computer that calculates a vector obtained by updating the state vector is executed,
The process of calculating the estimated state vector sets the estimated first vector to a value equal to the first vector,
The processing for calculating the covariance of the estimation error of the estimated state vector includes the component representing the covariance of the estimation error of the first vector in the covariance of the estimation error of the state vector, and the state in the state transition model. Among the process noise covariances generated in the vector, a component representing the process noise covariance generated in the first vector is added to estimate the estimated first vector out of the covariances of the estimated state vector estimation errors. A control program for a Kalman filter, characterized in that a component representing an error covariance is calculated.

Images