JP5141098B2 - Attitude data filter device, attitude data filtering method, and attitude data filter program. - Google Patents

Description

  The present invention relates to an attitude data filter device, an attitude data filtering method, and an attitude data filter program, and more particularly to a technique for smoothing time-series attitude data.

2. Description of the Related Art Conventionally, there has been known a method of using, for example, a three-dimensional geomagnetic sensor and a three-dimensional acceleration sensor and deriving posture data indicating the posture of an object on which these are mounted (for example, see Patent Document 1). According to this method, noise is mixed in the posture data, and when an impact is applied to the object, the posture is significantly different from the actual posture due to the use of the acceleration sensor to derive the posture. Data will be derived. As described above, it is not preferable to display the posture of the apparatus using the posture data mixed with noise or the wrong posture data as it is. Also, when performing some processing (for example, step count calculation processing) based on posture data, it is not preferable to use posture data mixed with noise or wrong posture data as it is. For this reason, conventionally, processing for smoothing posture data is performed by filter processing applied to acoustic data, image data, and the like.
International Publication No. WO / 2006-009247 Pamphlet

By the way, the posture of the object is in a state of changing continuously and cyclically. In addition, physical quantities such as Euler angles that are manipulated by conventional filtering are represented as a finite number of data sets defined in a linear space. As a Euler angle, as a set with a singular interval in which two consecutive postures corresponding to discontinuous data, that is, two poses that are not very small, are indicated by data with a large difference in a linear space, Assume that posture data indicating the posture of an object (for example, a portable information device) with no limitation on the rotation angle is defined. Such attitude data is regarded as data indicating continuous and non-circular physical quantities, that is, data corresponding to actual physical phenomena can be ignored, and smoothed by mathematical operations such as moving average. When the conventional filter processing is applied to the posture data, there is a problem that an appropriate processing result cannot be obtained in the singular section.
Further, when a filter device that performs smoothing by obtaining a moving average is realized as an FIR (Finite Impulse Response) filter device, there is a problem that a large number of buffer stages must be designed to increase the smoothing strength.

  The present invention provides an attitude data filter device, an attitude data filtering method, and an attitude data filter program that have no singular interval from which an inappropriate smoothing result is derived and that can increase the smoothing strength even when the number of buffer stages is small. The purpose is to provide.

  The inventor does not simply operate the posture data as mathematical data, but pays attention to the physical phenomenon indicated by the posture data, and expresses the angular velocity and angular acceleration inherent in the discrete posture data group in the time axis direction. And by modeling the filter process as an action of accelerating the rotation of the virtual object in the second posture indicated by the output data toward the first posture indicated by the input data, an inappropriate smoothing result is obtained. We succeeded in realizing the filtering without derived singular interval.

[1] That is, the attitude data filter device for achieving the above object receives input data A ′ _i indicating the first attitude at time iT (T is a constant, i = 0, 1, 2,...). Input means for sequentially acquiring;
Output data indicating the second posture at time iT: output means for sequentially outputting A _i ;
The angle of rotation from the second posture at time (i-1) T to the first posture at time iT is θ _i , the angular velocity at time iT indicated by the locus of the output data is ω _i , and time (i− 1) The angular acceleration of rotation of the second posture from T to time iT is
N _i = N _1i -N _2i
And when
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
Computing means for deriving A _{i based} on the input data, the initial value A ₀ and the initial value ω ₀ ;
Is provided.

1, 2 and 3 are explanatory diagrams showing models used in the present invention. An object whose input data A ′ _i indicates the first posture is referred to as a target object, and a virtual object whose output data A _i indicates the second posture is referred to as a virtual object. Now, it is assumed that the input data A ′ _i changes at the times (m−1) T, mT, and (m + 1) T in the order shown in FIGS. Since the filtering process is performed so that the attitude of the virtual object follows the attitude of the target object, it is assumed that the target object and the virtual object are rotated at a specific angular velocity at each time. Assuming the virtual object is rotated at a time (m-2) times from T (m-1) of constant period until T angular velocity ω _m-1, the angular velocity omega _m-1 of the virtual object as shown in FIG. 2 It can be derived from the trajectories of the virtual object posture data A _m-2 and A _m−1 .

In order to realize the smoothing function, the angular acceleration N of the virtual object may be determined so as to satisfy the following condition.
The rotation of the virtual object is accelerated so as to approach the posture indicated by the input data A ′ from the posture indicated by the output data A.
-The posture of the virtual object changes more slowly than the target object.
When the difference between the attitude indicated by the output data A and the attitude indicated by the input data A ′ becomes smaller, the angular velocity ω of the virtual object becomes closer to the angular velocity indicated by the locus of the input data A ′.

For satisfying these conditions represent the angular acceleration _{N i} of the virtual object as the difference between the _{N 1i} and _{N 2i, N} 1i illustrated in FIG. _3, defining the _{N 2i} as follows, respectively.
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture to the first posture.

N ₁ is a component that accelerates the rotation of the virtual object so that the posture indicated by the output data A approaches the posture indicated by the input data A ′ at an arbitrary time satisfying −π / 2 ≦ θ _i ≦ π / 2. In addition, it is a component that increases as the absolute value of θ indicating the difference between the attitude indicated by the output data A and the attitude indicated by the input data A ′ increases.

Here, it is assumed that the target object is stationary after the target object is rotated from the time at which both the target object and the virtual object are in the same posture and are stationary. However, if the virtual object is accelerated by the angular acceleration component N _1i , the component corresponding to the integral value of the acceleration component N _1i does not disappear from the angular velocity of the virtual object even if the target object is stationary again. The posture of the object does not converge to the posture of the target object indefinitely.

Therefore, an acceleration component N _2i that becomes a resistance to rotation of the virtual object is introduced. -N _2i increases as the virtual object rotates faster, and indicates an angular acceleration having a component in the direction opposite to the direction of rotation of the virtual object. That is,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
The introduction of such an angular acceleration component N _2i not only causes the virtual object to change its posture more gently than the target object, but also reduces the difference between the posture indicated by the output data A and the posture indicated by the input data A ′. Then, the angular velocity ω of the virtual object is close to the angular velocity indicated by the locus of the output data A.

Then, the initial value A ₀ of the posture of the virtual object and the initial value ω ₀ of the angular velocity of the target object are set, and the virtual object is expressed by the angular acceleration N _i = N _1i −N _2i of the virtual object represented by N _1i and N _2i. so it accelerates and sequentially operating the a _i indicating the posture of the virtual object, smoothing the orientation data is realized.
Since these operations can be performed by calculating a recurrence formula between adjacent binomials, the smoothing strength can be increased by setting the coefficient even if the number of buffer stages is small. Note that θ is an angle of rotation that rotates with the shortest path from the posture of the virtual object to the posture of the target object.

Further, the matrix indicating the rotation from the virtual object posture to the target object posture can be represented by R shown by the following equation (7). At this time, the vector ω ′ having the direction of the axis of rotation from the posture of the virtual object to the posture of the target object can be expressed by the following equation (8), and the rotation angle θ is a right-handed screw about ω ′. If the direction is positive, it can be expressed by the following equations (9) and (10). However, all vectors are column vectors, and row vectors are expressed as transposed matrices of column vectors.

前述のように、
（１）θ_ｉ＝０であるとき、Ｎ_１ｉ＝０であり、
（２）Ｎ_１ｉが示す角加速度の大きさはｓｉｎθ_ｉの絶対値に比例し、
（３）Ｎ_１ｉが示す角加速度の向きは時刻（ｉ−１）Ｔにおける前記第二の姿勢から時刻ｉＴにおける前記第一の姿勢への回転の向きに一致するとき、
Ｎ_１ｉ＝Ｋω"ｓｉｎθ_ｉ・・・（１２）
とかける。式（１２）は式（１１）の定義から、
Ｎ_１ｉ＝Ｋω'_ｉ・・・（１３）
となる。したがって、（２）Ｎ_１ｉが示す角加速度の大きさがｓｉｎθ_ｉの絶対値に比例するモデルは、計算量の低減の観点からも好ましい。 Here, ω ″ obtained by normalizing the magnitude of ω ′ by 1 is introduced by the following equation (11).

As aforementioned,
(1) When θ _i = 0, N _1i = 0,
(2) The magnitude of the angular acceleration indicated by N _1i is proportional to the absolute value of sin θ _i ,
(3) When the direction of angular acceleration indicated by N _1i coincides with the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
N _1i = Kω "sinθ _i (12)
Call it. Equation (12) is derived from the definition of Equation (11).
N _1i = Kω ′ _i (13)
It becomes. Therefore, (2) a model in which the magnitude of angular acceleration indicated by N _1i is proportional to the absolute value of sin θ _i is preferable from the viewpoint of reducing the amount of calculation.

Further, the direction of angular acceleration indicated by N _1i is matched with the direction of rotation from the second attitude at time (i−1) T to the first attitude at time iT, and the direction of angular acceleration indicated by N _2i is ω. By matching the direction of the angular velocity indicated by _i-1 , it is possible to maximize the efficiency of the acceleration operation in which the posture of the virtual object approaches the posture of the target object.

ただし、前記出力データが示す姿勢の回転は角速度ベクトルω_ｉを回転軸とする右ねじ方向への回転とし、||ω_ｉ||は角速度ω_ｉの大きさに比例し、
ｅ：ネイピア数
Γ、Ｋ：正の定数
Ｒ_ｍのｉ行ｊ列成分をｒ_ｍ，ｉｊとするとき、

である。 [3] Further, in the attitude data filter device for achieving the above-mentioned object, the input data and the output data are components of a strict orthogonal matrix, and the arithmetic means calculates the following equations (1) and (2) It is preferable to derive A _i using the chemical formula.

However, the rotation of the posture indicated by the output data is rotation in the right-handed direction with the angular velocity vector ω _i as the rotation axis, and || ω _i || is proportional to the magnitude of the angular velocity ω _i ,
e: Napier number Γ, K: When i row j column component of positive constant R _m is rm _{, ij} ,

It is.

First, consider formulating the model [2] in continuous time. The strict orthogonal matrix representing the posture of the object at time t is A (t), and at that time, the object is rotating in the right-handed direction with the angular velocity ω (t) represented by the following equation (14) as the rotation axis. . However, || ω (t) || is proportional to the magnitude of the angular velocity ω (t).

とすると、Ａ（ｔ）は次式（１６）の微分方程式を満たす。

At this time,

Then, A (t) satisfies the following differential equation (16).

In Equation (16), assuming that Ω (t) = Ω ₀ and constant, posture matrix A (t + T) when time T has elapsed from time t (a strict orthogonal matrix indicating the posture is referred to as a posture matrix). The following equation (17) can be written.

Therefore, when the time T is considered to be sufficiently small, the differential equation of the equation (16) can be approximated by the following equation (18).

Next, consider formulating the relationship between angular velocity and angular acceleration in continuous time. When the angular acceleration of ω (t) is N (t), the following equation (19) is established.

ここで、

である。またＲ_ｍは時刻（ｉ−１）Ｔにおける仮想物体の姿勢（第二の姿勢）から時刻ｉＴにおける対象物体の姿勢（第一の姿勢）への回転を示す正格直交行列である。 The following equation (20) shows the model of [2] as a differential equation in continuous time.

here,

It is. R _m is a strict orthogonal matrix indicating the rotation from the posture of the virtual object (second posture) at time (i−1) T to the posture of the target object (first posture) at time iT.

Considering that the time T is sufficiently small, the differential equation of the equation (20) can be approximated by the following equation (23).

である。
また、フィルタ処理の対象となる入力データ及び出力データを正格直交行列の成分とすることにより、特異区間におけるアルゴリズムを変更するための分岐処理などで処理が複雑化することがない。正格直交行列である姿勢行列によって姿勢を表現すると、微小な姿勢変化は、常に、行列成分の微小な変化として表れるからである。すなわち、姿勢行列の成分は、特異区間のないパラメータである。 By approximating the differential equations (16), (20) in continuous time by the equations (18), (23), the recurrence formula for deriving the output data A _i from the input data A ′ _i is given by the equation (1) ), (2), and (3), the amount of calculation is relatively small. In equation (2),

It is.
In addition, by using input data and output data to be filtered as components of a strict orthogonal matrix, processing is not complicated by branch processing for changing an algorithm in a singular section. This is because if the posture is expressed by a posture matrix that is a strict orthogonal matrix, a small change in posture always appears as a small change in matrix components. That is, the component of the posture matrix is a parameter without a singular interval.

The invention described so far can be applied not only to the posture data filter device but also to the posture data filtering method and the posture data filter program.
[4] In other words, the attitude data filtering method for achieving the above object is based on input data A ′ _i indicating the first attitude at time iT (T is a constant, i = 0, 1, 2,...). Obtain sequentially,
Output data indicating the second posture at time iT: A _i is sequentially output,
The angle of rotation from the second posture at time (i-1) T to the first posture at time iT is θ _i , the angular velocity at time iT indicated by the locus of the output data is ω _i , and time (i− 1) The angular acceleration of rotation of the second posture from T to time iT is
N _i = N _1i -N _2i
And when
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
A _i is derived based on the input data, the initial value A ₀ and the initial value ω ₀ .
Including that.

[5] Further, the attitude data filter program for achieving the above object includes the input data A ′ _i indicating the first attitude at time iT (T is a constant, i = 0, 1, 2,...). Input means for sequentially acquiring;
Output data indicating the second posture at time iT: output means for sequentially outputting A _i ;
The angle of rotation from the second posture at time (i-1) T to the first posture at time iT is θ _i , the angular velocity at time iT indicated by the locus of the output data is ω _i , and time (i− 1) The angular acceleration of rotation of the second posture from T to time iT is
N _i = N _1i -N _2i
And when
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
Derivation means for deriving A _{i based} on the input data, the initial value A ₀ and the initial value ω ₀ ;
And make the computer work.

  The order of the operations described in the claims is not limited to the order of description as long as there is no technical obstruction factor, and may be executed at the same time, may be executed in the reverse order of the description order, or may be continuous. It does not have to be executed in order. The function of each means described in the claims is realized by hardware resources whose function is specified by the configuration itself, hardware resources whose function is specified by a program, or a combination thereof. The functions of these means are not limited to those realized by hardware resources that are physically independent of each other. Furthermore, the present invention is also established as a recording medium for the attitude data filter program. Of course, the recording medium for the computer program may be a magnetic recording medium, a magneto-optical recording medium, or any recording medium that will be developed in the future.

Hereinafter, embodiments of the present invention will be described in the following order with reference to the accompanying drawings. In addition, the same code | symbol is attached | subjected to the corresponding component in each figure, and the overlapping description is abbreviate | omitted.
＊＊＊＊＊＊＊＊＊＊＊＊＊＊＊
1. 1. Hardware configuration of attitude data filter device Posture matrix 3. Software configuration of attitude data filter device 4. Attitude data filtering processing Other Embodiments ***********

[1. Hardware configuration of attitude data filter device]
FIG. 4 is a block diagram showing an embodiment of the attitude data filter device according to the present invention. In FIG. 4, the attitude data filter device is represented as the control unit 13 of the portable information device 10. The portable information device 10 includes an acceleration sensor 11, a geomagnetic sensor 12, a control unit 13, and a display 14. The portable information device 10 is a portable information processing device that processes posture data, such as a PDA (Personal Digital Assistance), a portable phone, a portable game machine, a pedometer, and an electronic compass.

  The acceleration sensor 11 may be of any detection method such as a piezoresistive type, a capacitance type, or a heat detection type, and acceleration in the direction opposite to the gravitational acceleration is obtained by three axis components of x, y, and z orthogonal to each other. And acceleration data, which is vector data indicating acceleration obtained by combining the acceleration inherent to the motion of the acceleration sensor. Since the direction of the acceleration data output from the acceleration sensor 11 in a stationary state indicates the direction of gravity, the acceleration data can be used as data indicating the inclination of the portable information device 10.

The geomagnetic sensor 12 includes three magnetic sensor units composed of MI elements, MR elements, and the like, and outputs geomagnetic data, which is vector data indicating the direction of geomagnetism by three axes of x, y, and z that are orthogonal to each other. By combining the acceleration data and the geomagnetic data, it is possible to accurately display the orientation viewed from the portable information device 10.
The control unit 13 is a computer including a processor, a storage medium (for example, a RAM and a ROM), and an interface (not shown), and functions as an input unit, an output unit, and a calculation unit by executing an attitude data filter program described later.

[2. (Attitude matrix)
Here, an attitude matrix as attitude data indicating the attitude of the portable information device 10 will be described. For example, it is assumed that the attitude of the portable information device 10 is expressed by a coordinate system based on a horizontal eastward vector a, a horizontal northward vector b, and a vertically upward vector c shown in FIG. The vectors a, b, and c are defined by the following equation (27) so that these vectors become reference bases.

A vector x, y, z having a direction of three reference axes x, y, z fixed to the portable information device 10 and perpendicular to each other and having a size of 1 is represented by the reference base {a, b, c}. Expressing as a linear combination is equivalent to expressing the attitude of the portable information device 10. Therefore, the vectors x, y, z are converted into coefficients x _a , x _b , x _c , y _a as linear combinations of the reference bases {a, b, c} as in the following equations (28), (29), (30). , and it expressed using _{y b, y c, z a} , z b, z c.

基準基底を式（２７）で定義しているため、式（３１）は次式（３２）と等価である。

The relationship between the equations (28), (29), and (30) can be rewritten as the following equation (31).

Since the reference basis is defined by Expression (27), Expression (31) is equivalent to the following Expression (32).

A matrix of the following equation (33) in which the vectors x, y, and z are collectively expressed as one matrix is referred to as an attitude matrix. The matrix A is a strict orthogonal matrix whose determinant is 1, and corresponds one-to-one with the attitude of the mobile information device 10 with 3 degrees of freedom. When the posture is expressed by such a posture matrix, a minute posture change always appears as a minute change of the matrix component. That is, the component of the posture matrix is a parameter without a singular interval.

[3. Software configuration of attitude data filter device]
The control unit 13 has a function of displaying the attitude of the portable information device 10 on the display 14 by executing a computer program including the module group shown in FIG.
The acceleration data processing module 131 inputs acceleration data output from the acceleration sensor 11 at a time interval T ′, and performs a predetermined correction process on the acceleration data.
The geomagnetic data processing module 132 inputs the geomagnetic data output from the geomagnetic sensor 12 at a time interval T ′, and performs a predetermined correction process on the geomagnetic data.
The attitude data generation module 133 generates attitude data that is a component of the input attitude matrix A ′, which is an attitude matrix, based on the acceleration data and the geomagnetic data. Since the process of generating the attitude data based on the acceleration data and the geomagnetic data is a well-known process, the description thereof is omitted.

The attitude data filter module 134 is a program that performs a filtering process on the input attitude matrix A ′ _i at time iT, and outputs a component of the output attitude matrix A _i that is an attitude matrix indicating the attitude of the portable information device 10 as output data. is there. The output attitude matrix A is derived based on the initial values A ₀ and ω ₀ and the input attitude matrix A ′ _i stored in the nonvolatile storage medium of the control unit 13.
The attitude data processing module 135 executes a process for displaying the attitude of the portable information device 10 on the display 14 based on the output attitude matrix A _i . The posture of the portable information device 10 may be displayed as a number such as Euler angle or may be displayed as an image.

[3. (Attitude data filtering process)
FIG. 6 is a flowchart showing posture data filtering processing by the posture data filter module 134.
First, the attitude data filter module 134 inputs an input attitude matrix A ′ _m (m = 0, 1, 2,...) (Step S10).

Next, the attitude data filter module 134 derives R _m from the input attitude matrix A ′ _m and the output attitude matrix A _m−1 (step S11). R _m indicates the rotation from the posture of the virtual object (second posture) at time (m−1) T to the posture of the target object (first posture) at time mT as shown in the following equation (34). It is a strict orthogonal matrix.

Next, the attitude data filter module 134 derives an angular velocity ω ′ _m of rotation from the second attitude at time (m−1) T to the first attitude at time mT from R _m (step S12). The rotation of the portable information device 10 is expressed as a rotation in the right-hand screw direction with the angular velocity vector as the rotation axis, and || ω ′ _i || is proportional to the magnitude of the angular velocity ω ′ _i , and i row j of R _m When the column component is rm _{, ij} , ω ' _i is determined by the following equation (35). K is a positive constant.

またΩ_ｉは次式（３８）によって定まる。

Next, the attitude data filter module 134 derives ω _m and Ω _m from ω ′ _m−1 and ω _m−1 (step S13). ω _i is an angular velocity at the time iT of the portable information device 10 indicated by the locus of the output attitude matrix, and is determined by the following equations (36) and (37).

Ω _i is determined by the following equation (38).

姿勢データフィルタモジュール１３４は、以上の処理を入力姿勢行列Ａ'_ｉ毎に順次繰り返すことにより、出力姿勢行列Ａ_ｉの成分を姿勢データとして順次姿勢データフィルタ処理モジュール１３５に出力する。
以上説明した実施形態によると、フィルタ処理の対象となる入力データ及び出力データを正格直交行列の成分とすることにより、特異区間におけるアルゴリズムを変更するための分岐処理などで処理が複雑化することがない。連続時間における微分方程式（１６）、（２０）を式（１８）、（２３）によって近似することによって、入力データＡ'_ｉから出力データＡ_ｉを導出するための計算量を低減することができる。また、隣接二項間漸化式の演算によって入力データＡ'_ｉから出力データＡ_ｉを導出できるため、演算部１３に必要なバッファの段数を低減することができる。また、定数ＫおよびΓの設定によって、平滑化強度を自由に設定することができる。 Next, the posture data filter module 134 derives a component of the output posture matrix A _m from the output posture matrix A _{m −1} , Ω _m (step S14). The output attitude matrix _Am is determined by the following equation (39).

The attitude data filter module 134 sequentially outputs the components of the output attitude matrix A _{i to} the attitude data filter processing module 135 as attitude data by sequentially repeating the above processing for each input attitude matrix A ′ _i .
According to the embodiment described above, the input data and the output data to be subjected to the filtering process are components of the strict orthogonal matrix, so that the process becomes complicated by a branch process for changing the algorithm in the singular section. Absent. By approximating the differential equations (16) and (20) in continuous time with the equations (18) and (23), the amount of calculation for deriving the output data A _i from the input data A ′ _i can be reduced. . Further, since the output data A _i can be derived from the input data A ′ _i by the calculation of the recurrence formula between adjacent binomials, the number of buffer stages required for the calculation unit 13 can be reduced. Further, the smoothing strength can be freely set by setting the constants K and Γ.

[4. Other embodiments]
The technical scope of the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the scope of the present invention. For example, the time interval T may be sufficiently short as long as it meets the specifications. The time interval T ′ for inputting acceleration data and geomagnetic data to the control unit 13 and the time interval T used in the recurrence formula of the attitude data filter are as follows. They may coincide with each other, or T = nT ′ (n = 1, 2,...), T = T ′ / n.

  The format of the input data and output data indicating the posture may be any form that can represent the posture of the target object, and a quaternion in which a minute posture change always appears as a minute parameter change may be used. Alternatively, an Euler angle having a singular section in which a minute posture change appears as a discontinuous change in parameters may be used.

The output attitude matrix A _i represents the angle of rotation from the second attitude at time (i−1) T to the first attitude at time iT as θ _i , and the angular velocity at time iT indicated by the locus of output data as ω. _i , When the angular acceleration of the rotation of the second posture from time (i-1) T to time iT is N _i = N _1i −N _2i , the following condition may be satisfied.
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
That is, for example, the function of θ satisfying (2) and the magnitude of N _1i may be functions as shown in FIGS. 7A and 7B, and the direction of angular acceleration indicated by N _1i is the time ( i-1) The direction of rotation from the second posture at T to the first posture at time iT may be slightly deviated, and the direction of angular acceleration indicated by N _2i is slightly different from the direction of angular velocity indicated by ω _i. It may shift.

Explanatory drawing concerning embodiment of this invention. Explanatory drawing concerning embodiment of this invention. Explanatory drawing concerning embodiment of this invention. The block diagram concerning embodiment of this invention. The schematic diagram concerning embodiment of this invention. The flowchart concerning embodiment of this invention. The graph concerning embodiment of this invention.

Explanation of symbols

10: Portable information device, 11: Acceleration sensor, 12: Geomagnetic sensor, 13: Control unit, 14: Display, 131: Geomagnetic data processing module, 132: Acceleration data processing module, 133: Posture data generation module, 134: Posture Data filter module 135: Attitude data processing module

Claims (4)

Input means for sequentially acquiring input data A ′ _i indicating the first posture at time iT (T is a constant, i = 0, 1, 2,...);
Output data indicating the second posture at time iT: output means for sequentially outputting A _i ;
The angle of rotation from the second posture at time (i-1) T to the first posture at time iT is θ _i , the angular velocity at time iT indicated by the locus of the output data is ω _i , and time (i− 1) The angular acceleration of rotation of the second posture from T to time iT is
N _i = N _1i -N _2i
And when
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
Computing means for deriving A _{i based} on the input data, the initial value A ₀ and the initial value ω ₀ ;
An attitude data filter device comprising: The input data and the output data are components of a strict orthogonal matrix;
The calculation means derives A _i using a recurrence formula of the following formulas (1), (2), and (3):
The attitude data filter device according to claim 1 .

However ,
e: Napier number Γ, K: When i row j column component of positive constant R _m is rm _{, ij} ,

It is. Input data A ′ _i indicating the first posture at time iT (T is a constant, i = 0, 1, 2,...) Are sequentially acquired.
Output data indicating the second posture at time iT: A _i is sequentially output,
The angle of rotation from the second posture at time (i-1) T to the first posture at time iT is θ _i , the angular velocity at time iT indicated by the locus of the output data is ω _i , and time (i− 1) The angular acceleration of rotation of the second posture from T to time iT is
N _i = N _1i -N _2i
And when
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
A _i is derived based on the input data, the initial value A ₀ and the initial value ω ₀ .
Posture data filtering method. Input means for sequentially acquiring input data A ′ _i indicating the first posture at time iT (T is a constant, i = 0, 1, 2,...);
Output data indicating the second posture at time iT: output means for sequentially outputting A _i ;
The angle of rotation from the second posture at time (i-1) T to the first posture at time iT is θ _i , the angular velocity at time iT indicated by the locus of the output data is ω _i , and time (i− 1) The angular acceleration of rotation of the second posture from T to time iT is
N _i = N _1i -N _2i
And when
(1) When θ _i = 0, N _1i = 0,
(2) When −π / 2 ≦ θ _i ≦ π / 2, the magnitude of angular acceleration indicated by N _1i increases monotonously with respect to the absolute value of θ _i ,
(3) The direction of angular acceleration indicated by N _1i corresponds to the direction of rotation from the second posture at time (i−1) T to the first posture at time iT,
(4) The magnitude of the angular acceleration indicated by N _2i monotonously increases with respect to the magnitude of the angular velocity indicated by ω _i ,
(5) The direction of angular acceleration indicated by N _2i corresponds to the direction of angular velocity indicated by ω _i .
Derivation means for deriving A _{i based} on the input data, the initial value A ₀ and the initial value ω ₀ ;
Posture data filter program that allows the computer to function.

Images