METHOD AND APPARATUS FOR CALCULATION OF ANGULAR VELOCITY USING ACCELERATION SENSOR AND GEOMAGNETIC SENSOR

Abstract

A processor-implemented method based on an acceleration sensor and a geomagnetic sensor for determining an angular velocity of an object includes: deriving a state variable and a variance of the state variable based on an error quaternion and converting a quaternion-based rotation matrix into an error quaternion-based rotation matrix; calculating an observation matrix and an output matrix of the Kalman filter based on the error quaternion-based rotation matrix; calculating a gain of the Kalman filter based on the transferred variance of the state variable and the observation matrix of the Kalman filter after transferring the state variable and the variance of the state variable through a discretized transfer matrix; calculating a quaternion-based on a calibrated state variable and an estimated quaternion after calibrating the state variable and the variance of the state variable through the gain of the Kalman filter; and calculating angular velocity based on the quaternion.

Description

CROSS-REFERENCE TO RELATED APPLICATIONS

This application claims under 35 USC 119(a) of Korean Patent Application No. 10-2017-0071721 filed on Jun. 8, 2017, in the Korean Intellectual Property Office, the entire disclosure of which is incorporated herein by reference for all purposes.

BACKGROUND

1. Field

The present disclosure relates to a method and an apparatus for calculating angular velocity, and more particularly, to a method and an apparatus for calculating angular velocity using measurement values of an acceleration sensor and a geomagnetic sensor.

2. Description of the Related Art

Angular velocity information is utilized in various fields in which motion information of an object in motion is required. Angular velocity (or rotational angular velocity) is used to calculate a traveling direction and a rotating direction required for navigational systems used in vehicles, e.g., automobiles, marine vessels, etc. Angular velocity may be used to determine and control a traveling direction and orientation of aerial vehicles or autonomous vehicles that are now becoming ubiquitous. Angular velocity calculations may also be utilized in a game controller of a game machine or a direction input device of a user in an augmented reality field, and further utilized to control angular positions and recognize motion in a field of a game compass using a mobile phone as an input device. The tendency to require angular velocity information as described above is ever increasing.

An angular velocity calculating device is used to measure rotation angle information and position information when an object moves in a space. To this end, a physical gyro sensor which acquires rotational acceleration may be used.

The gyro sensor may use various principles to calculate angular velocity information, but mainly uses Coriolis force. When an object rotates and travels, the Coriolis force is generated at a direction of 90° with respect to the traveling direction and is generated only by the rotation. Generally, since it is not easy to directly measure the angular velocity, the Coriolis force is measured first and then converted to measure the angular velocity.

However, in order to implement the measuring principle, a detecting structure needs to be rotated or vibrated in the sensor, which becomes an obstacle to minimize the sensor. Such characteristics also cause the same problems in a micro-electro mechanical system MEMS gyro sensor. Most MEMS gyro sensors use a tuning fork method, a vibration wheel method, a Foucault pendulum method, or a wine glass tuning method, which also need to include a detecting structure which does not stop, but moves in the sensor.

Further, since a structure that maintains a state of motion in a relatively small space is required by the sensor, there is a limit to miniaturizing the device, which results in an increase in the level of difficulty of design, cost and manufacturing. In order to maintain the detecting structure in a motion state, the detecting structure needs to be electrically excited. Therefore, power consumption of the gyro sensor is relatively higher than that of other sensors. In this case, an internal temperature of the sensor is also increased due to the consumed current, which damages electrical stability. Further, the internal structure is complicated so that the sensor is vulnerable to a mechanical impact.

Even though the gyro sensor outputs angular velocity due to its characteristic, in fact, in various applications, an angle is more frequently used than the angular velocity. In order to convert the angular velocity into angle information, an integration process needs to be performed. However, the integration process causes mathematical instability to generate mathematical saturation and cause an erroneous operation thereby. All sensors inevitably include offsets and the gyro sensor is not exceptional and allows offsets as specified in a product specification. Even though the offset is small, the saturation phenomenon cannot be avoided unless the offset value is periodically reset as a sufficient time passes. This characteristic also limits the application.

SUMMARY

This Summary is provided to introduce a selection of concepts in a simplified form that are further described below in the Detailed Description. This Summary is not intended to identify key features or essential features of the claimed subject matter, nor is it intended to be used as an aid in determining the scope of the claimed subject matter.

In one general aspect, a processor-implemented method based on an acceleration sensor and a geomagnetic sensor for determining an angular velocity of an object includes: deriving a state variable and a variance of the state variable based on an error quaternion and converting a quaternion-based rotation matrix into an error quaternion-based rotation matrix; calculating an observation matrix and an output matrix of the Kalman filter after redefining an output of an acceleration sensor and an output of a geomagnetic sensor using the error quaternion-based rotation matrix; calculating a gain of the Kalman filter based on the transferred variance of the state variable and the observation matrix of the Kalman filter after transferring the state variable and the variance of the state variable through a discretized transfer matrix; calculating a quaternion-based on a calibrated state variable and an estimated quaternion after calibrating the state variable and the variance of the state variable through the gain of the Kalman filter; and calculating the angular velocity based on the quaternion.

In the converting of a quaternion-based rotation matrix into an error quaternion-based rotation matrix, an error quaternion [q_ex, q_ey, q_ez]′ of a quaternion q=[q₀, q_x, q_y, q_z]′ which expresses a relative angle change between a reference coordinate system and a sensor coordinate system may be set to a state variable x.

In the converting of a quaternion-based rotation matrix into an error quaternion-based rotation matrix, when a current state and a previous state have a statistic correlation in which only time delay τ is generated, the state variable x may be expressed by an equation of {dot over (x)}=Fx, where F is a transfer matrix expressed by

$F=-\frac{1}{\tau }\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$

and τ is a time factor representing an associated degree between the states.

In the converting of a quaternion-based rotation matrix into an error quaternion-based rotation matrix, a variance of the state variable may be calculated by an equation of {circumflex over (P)}(t)=E{(x(t)−{circumflex over (x)}(t))(x(t)−{circumflex over (x)}(t))′}, where x is a state variable, {circumflex over (x)} is an estimated state variable, and ′ is a transpose matrix.

In the converting of a quaternion-based rotation matrix into an error quaternion-based rotation matrix, the quaternion-based rotation matrix may be converted into the error quaternion-rotation matrix based on an equation of C(q)=C(q_e)C({circumflex over (q)}), the quaternion-based rotation matrix is

$C\left(q\right)=\left[\begin{array}{ccc}2q_0^2+2q_x^2-1& 2q_xq_y+2q_0q_z& 2q_xq_z-2q_0q_y\\ 2q_xq_y-2q_0q_z& 2q_0^2+2q_y^2-1& 2q_yq_z+2q_0q_x\\ 2q_xq_z+2q_0q_y& 2q_yq_z-2q_0q_x& 2q_0^2+2q_z^2-1\end{array}\right],$

the error quaternion-based rotation matrix is

$C\left(q_e\right)\approx \left[\begin{array}{ccc}1& 2q_{\mathrm{ez}}& -2q_{\mathrm{ey}}\\ -2q_{\mathrm{ez}}& 1& 2q_{\mathrm{ex}}\\ 2q_{\mathrm{ey}}& -2q_{\mathrm{ex}}& 1\end{array}\right]=I_{3\times 3}-2q_3\times ,$

where q_ex, q_ey, q_ez are vector component factors of the error quaternion, q_ex represents

$\left[\begin{array}{ccc}0& -q_{\mathrm{ez}}& q_{\mathrm{ey}}\\ q_{\mathrm{ez}}& 0& -q_{\mathrm{ex}}\\ -q_{\mathrm{ey}}& q_{\mathrm{ex}}& 0\end{array}\right],$

and q₀ is a factor corresponding to

$\cos \left(\frac{\theta }{2}\right)$

scalar in the quaternion.

In the converting of a quaternion-based rotation matrix into an error quaternion-based rotation matrix, when q_ex, q_ey, and q_ez are minute amounts, the error quaternion based rotation matrix expressed by

$C\left(q_e\right)=\left[\begin{array}{ccc}1+2q_{\mathrm{ex}}^2& 2q_{\mathrm{ex}}q_{\mathrm{ey}}+2q_{\mathrm{ez}}& 2q_{\mathrm{ex}}q_{\mathrm{ez}}-2q_{\mathrm{ey}}\\ 2q_{\mathrm{ex}}q_{\mathrm{ey}}-2q_{\mathrm{ez}}& 1+2q_{\mathrm{ey}}^2& 2q_{\mathrm{ey}}q_{\mathrm{ez}}+2q_{\mathrm{ex}}\\ 2q_{\mathrm{ex}}q_{\mathrm{ez}}+2q_{\mathrm{ey}}& 2q_{\mathrm{ey}}q_{\mathrm{ez}}-2q_{\mathrm{ex}}& 1+2q_{\mathrm{ez}}^2\end{array}\right]$

and may be simplified into

$C\left(q_e\right)\approx \left[\begin{array}{ccc}1& 2q_{\mathrm{ez}}& -2q_{\mathrm{ey}}\\ -2q_{\mathrm{ez}}& 1& 2q_{\mathrm{ex}}\\ 2q_{\mathrm{ey}}& -2q_{\mathrm{ex}}& 1\end{array}\right]=I_{3\times 3}-2q_e\times .$

In the calculating of an observation matrix and an output matrix of the Kalman filter, an acceleration sensor output Y_a and an output Y_m of the geomagnetic sensor may be redefined based on a correlation between the error quaternion q_e expressed by C(q)=C(q_e)C({circumflex over (q)})=(I−2q_ex)C({circumflex over (q)}) and the quaternion-based rotation matrix C(q), the acceleration sensor output Y_a is Y_a−C({circumflex over (q)}){right arrow over (G)}=2C({circumflex over (q)}){right arrow over (G)}q_e+B_a+V_a, the output Y_m of the geomagnetic sensor is Y_m−C({circumflex over (q)}){right arrow over (M)}=2C({circumflex over (q)}){right arrow over (M)}q_e+V_m, where C({circumflex over (q)}) is an estimated quaternion-based rotation matrix, I_3×3 is a 3×3 unit matrix, {right arrow over (G)} is a gravitational acceleration vector, B_a is a linear acceleration, V_a is a noise of the acceleration sensor, {right arrow over (M)} is an earth geomagnetic vector, and V_m is a noise of the geomagnetic sensor.

In the calculating of an observation matrix and an output matrix of the Kalman filter, based on the acceleration sensor output Y_a and the output Y_m of the geomagnetic sensor, an observation matrix of the Kalman filter of

$H=\left[\begin{array}{c}2C\left(\hat{q}\right)\overrightarrow{G}q_e\\ 2C\left(\hat{q}\right)\overrightarrow{M}q_e\end{array}\right]$

may be calculated and an output matrix of the Kalman filter of

$Z=\left[\begin{array}{c}{Y}_a-C\left(\hat{q}\right)\overrightarrow{G}\\ Y_m-C\left(\hat{q}\right)\overrightarrow{M}\end{array}\right]$

may be calculated.

In the calculating of a gain of the Kalman filter, the state variable x may be transferred according to an equation of {circumflex over (x)}_k+1=F_k{circumflex over (x)}_k and a variance p of the state variable may be transferred according to an equation of {circumflex over (P)}_k+1=F_k{circumflex over (P)}_kF′_k+Qd_k, where F_k is a discretized transfer matrix expressed by an equation of F_k≈I+F(kT)T+½F(kT)²T², Qd_k is a discretized process noise variance of Q expressed by an equation of Qd_k≈QT+½F (kT)+½F (kT)′, and T is a sampling time.

In the calculating of a gain of the Kalman filter, the discretized transfer matrix expressed by an equation of

$F_k=e^{F*T}=e^{\frac{-T}{\tau }}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

may be assumed to be a first-order linear formula to be simplified into F_k≈I+F(kT)T+½F(kT)²T², F is a transfer matrix expressed by

$F=-\frac{1}{\tau }\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$

and τ is a time factor which represents an associated degree between states.

In the calculating of a gain of the Kalman filter, the gain of the Kalman filter may be calculated according to an equation of K_k=P_kH′_k(H_k{circumflex over (P)}_kH′_k+R)⁻¹, where R is a variance of a measurement sensor error and is expressed by an equation of

$R=\left[\begin{array}{cc}{R}_aI_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& R_mI_{3\times 3}\end{array}\right],$

I_3×3 is a 3×3 unit matrix, 0_3×3 is a 3×3 0 matrix, R_a is a variance of the acceleration sensor, and R_m is a variance of the geomagnetic sensor.

In the calculating of a quaternion, the state variable may be calibrated according to an equation of x_k={circumflex over (x)}_k+K_k(Z_k−H_k{circumflex over (x)}_k) and a variance of the state variable may be calibrated according to an equation of P_k=(I−K_kH_k){circumflex over (P)}_k(I−K_kH_k)′+K_kRK′_k, where K_k is a gain of the Kalman filter, Z_k is an output matrix of the Kalman filter, H_k is an observation matrix of the Kalman filter, I is a 3×3 unit matrix, and R is a variance of a measurement sensor error.

In the calculating of a quaternion, after setting the calibrated state variable to be an error quaternion, the quaternion may be calculated according to an equation of q_k={circumflex over (q)}_k⊗[1 q_ex q_ey q_ez]′_k, {circumflex over (q)}_k is an estimated quaternion and are q_ex, q_ey, q_ez are three-factor variables of the error quaternion.

In the calculating of angular velocity based on the quaternion, after acquiring a differential expression expressed by an equation of

$\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}}=\frac{1}{2}W\left(t\right)q\left(t\right)$

by differentiating the quaternion, the differential expression may be arranged for every factor and then discretized to acquire an angular velocity calculating equation expressed by an equation of

$\left[\begin{array}{c}{\omega }_x^k\\ {\omega }_y^k\\ {\omega }_z^k\end{array}\right]=2/T\left[\begin{array}{c}{q}_0^k-q_0^{k-1}\\ q_x^k-q_x^{k-1}\\ q_y^k-q_y^{k-1}\\ q_z^k-q_z^{k-1}\end{array}\right]\left[\begin{array}{cccc}-q_x^k& q_0^k& q_z^k& q_y^k\\ -q_y^k& -q_z^k& q_0^k& q_x^k\\ -q_z^k& q_y^k& -q_x^k& q_0^k\end{array}\right],$

where W(t) is an angular velocity vector expressed by an equation of W(t)=[0 ω₀ ω_x ω_z]′, q₀, q_x, q_y, and q_z are four factors which configure the quaternion, and T is a sampling time.

In the calculating of angular velocity based on the quaternion, the differential expression may be arranged for every factor to derive and arrange equations of

${\hspace{0.17em}}^{“}\frac{{\mathrm{dq}}_0}{\mathrm{dt}}=-\frac{1}{2}{\left({\omega }_xq_x+{\omega }_yq_y+{\omega }_zq_z\right)}^{”},{\hspace{0.17em}}^{“}\frac{{\mathrm{dq}}_x}{\mathrm{dt}}=\frac{1}{2}{\left({\omega }_xq_0+{\omega }_yq_z-{\omega }_zq_y\right)}^{”},{\hspace{0.17em}}^{“}\frac{{\mathrm{dq}}_y}{\mathrm{dt}}=\frac{1}{2}{\left({\omega }_yq_0+{\omega }_zq_x-{\omega }_zq_z\right)}^{”},{\hspace{0.17em}}^{“}\frac{{\mathrm{dq}}_z}{\mathrm{dt}}=-\frac{1}{2}{\left({\omega }_zq_0+{\omega }_xq_y-{\omega }_xq_z\right)}^{”}$

to generate an equation for calculating an angular velocity vector expressed by an equation of

$W\left(t\right)=2\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}}{q\left(t\right)}^{-1}$

and then discretize the equation for calculating an angular velocity vector to acquire the angular velocity calculating equation, and q⁻¹ is expressed by an equation of q⁻¹=[q₀ −q_x −q_y −q_z]′.

The method may further include calculating an Euler angle by rotating the Euler angle in the order of a z axis, a y axis, and an x axis in the quaternion, in which the Euler angle is calculated based on at least one equation of Roll=tan⁻¹(2(q₀q_x+q_yq_z),1−2(q_x²+q_y²)), Pitch=sin⁻¹(2(q₀q_y −q_zq_x)), and Yaw=tan⁻¹(2(q₀q_z+q_xq_y), 1−2(q_y²+q_z²)), and q₀, q_x, q_y, and q_z are four factors which configure the quaternion.

The method may further include calculating an Euler rotational axis and angle based on the quaternion, in which the Euler rotational axis is calculated according to an equation of

$\hat{e}=\frac{\overrightarrow{q}}{\overrightarrow{q}},$

the angle of the Euler rotational axis may be calculated according to an equation of θ_e=2 cos⁻¹(q₀), where {right arrow over (q)} is a vector of a quaternion represented by {right arrow over (q)}=q_xi+q_yj+q_zk, i, j, and k are unit vectors of individual axes, and ∥{right arrow over (q)}∥ is an absolute value of {right arrow over (q)}.

The method may further include calculating linear acceleration of an object which is in motion based on the quaternion, in which the linear acceleration is calculated according to an equation of B_a=−Y_a C(q){right arrow over (G)}, Y_a is an acceleration sensor output, C(q) is a quaternion, and {right arrow over (G)} is a gravitational acceleration vector.

In another general aspect, an angular velocity calculating apparatus based on an acceleration sensor and a geomagnetic sensor for determining an angular velocity of an object includes an error quaternion setting device, an estimating device, a filter configuring device, a calibrating device, a quaternion calculating device, and an angular velocity calculator. The error quaternion setting device is configured to set an error quaternion. The estimating device is configured to derive a state variable and a variance of the state variable based on the error quaternion, and convert a quaternion-based rotation matrix into an error quaternion-based rotation matrix. The filter configuring device is configured to redefine an acceleration sensor output and an output of the geomagnetic sensor using the error quaternion-based rotation matrix to calculate an observation matrix and an output matrix of a Kalman filter, and transfer the state variable and a variance of the state variable through a discretized transfer matrix to calculate a gain of the Kalman filter. The calibrating device is configured to calibrate the state variable and the variance of the state variable through the gain of the Kalman filter. The quaternion calculating device is configured to calculate a quaternion based on the calibrated state variable and the estimated quaternion. The angular velocity calculator is configured to calculate the angular velocity based on the quaternion.

The estimating device may be configured to set an error quaternion [q_ex, q_ey, q_ez]′ of a quaternion q=[q₀, q_x, q_y, q_z]′ which expresses a relative angle change between a reference coordinate system and a sensor coordinate system to a state variable (x), when a current state and a previous state have a statistic correlation in which only time delay τ is generated, the state variable x may be expressed by an equation of {dot over (x)}=Fx, F may be a transfer matrix expressed by

$F=-\frac{1}{\tau }\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right],$

and τ may be a time factor representing an associated degree between the states.

The estimating device may be configured to convert the quaternion-based rotation matrix into the error quaternion-based rotation matrix based on an equation of C(q)=C(q_e))C({circumflex over (q)}), the quaternion-based rotation matrix is

$C\left(q\right)=\left[\begin{array}{ccc}2q_0^2+2q_x^2-1& 2q_xq_y+2q_0q_z& 2q_xq_z-2q_0q_y\\ 2q_xq_y-2q_0q_z& 2q_0^2+2q_y^2-1& 2q_yq_z+2q_0q_x\\ 2q_xq_z-2q_0q_y& 2q_yq_z-2q_0q_x& 2q_0^2+2q_z^2-1\end{array}\right],$

the error quaternion-based rotation matrix is

$C\left(q_e\right)\approx \left[\begin{array}{ccc}1& 2q_{\mathrm{ez}}& -2q_{\mathrm{ey}}\\ -2q_{\mathrm{ez}}& 1& 2q_{\mathrm{ex}}\\ 2q_{\mathrm{ey}}& -2q_{\mathrm{ex}}& 1\end{array}\right]=I_{3\times 3}-2q_e\times ,$

where q_ex, q_ey, and q_ez are vector component factors of the error quaternion, q_ex represents

$\left[\begin{array}{ccc}0& -q_{\mathrm{ez}}& q_{\mathrm{ey}}\\ q_{\mathrm{ez}}& 0& -q_{\mathrm{ex}}\\ -q_{\mathrm{ey}}& q_{\mathrm{ex}}& 0\end{array}\right],$

and q₀ is a factor corresponding to

$\cos \left(\frac{\theta }{2}\right)$

scalar in the quaternion.

The filter configuring device may be configured to redefine an output Y_a of the acceleration sensor and an output Y_m of the geomagnetic sensor based on a correlation between the error quaternion q_e expressed by C(q)=C(q_e)C({circumflex over (q)})=(I−2q_ex)C({circumflex over (q)}) and the quaternion-based rotation matrix C(q), the redefined acceleration sensor output Y_a is Y_a−C({circumflex over (q)}){right arrow over (G)}=2C({circumflex over (q)}){right arrow over (G)}q_e+B_a+V_a, the redefined output Y_m of the geomagnetic sensor is Y_m−C({circumflex over (q)}){right arrow over (M)}=2C({circumflex over (q)}){right arrow over (M)}q_e+V_m, C({circumflex over (q)}) is an estimated quaternion-based rotation matrix, I_3×3 is a 3×3 unit matrix, {right arrow over (G)} is a gravitational acceleration vector, B_a may be linear acceleration, V_a may be a noise of the acceleration sensor, {right arrow over (M)} may be an earth geomagnetic vector, and V_m may be a noise of the geomagnetic sensor.

The filter configuring device may be configured to calculate an observation matrix of the Kalman filter of

$H=\left[\begin{array}{c}2C\left(\hat{q}\right)\overrightarrow{G}q_e\\ 2C\left(\hat{q}\right)\overrightarrow{M}q_e\end{array}\right]$

and calculate an output matrix of the Kalman filter of

$Z=\left[\begin{array}{c}{Y}_a-C\left(\hat{q}\right)\overrightarrow{G}\\ Y_m-C\left(\hat{q}\right)\overrightarrow{M}\end{array}\right],$

based on the acceleration sensor output Y_a and the output Y_m of the geomagnetic sensor.

The filter configuring device may be configured to transfer the state variable x according to an equation of {circumflex over (x)}_k+1=F_k{circumflex over (x)}_k, transfer a variance p of the state variable according to an equation of {circumflex over (P)}_k+1=F_k{circumflex over (P)}_kF′_k+Qd_k, and then calculate a gain of the Kalman filter according to an equation of K_k=P_kH′_k(H_k{right arrow over (P)}_kH′_k+R)⁻¹, where F_k is a discretized transfer matrix represented by an equation of F_k≈I+F(kT)T+½F(kT)²T², Qd_k is a discretized process noise variance of Q represented by an equation of Qd_k≈QT+½F(kT)+½F(kT)′, where T is a sampling time, Z_k is an output matrix of the Kalman filter, H_k is an observation matrix of the Kalman filter, and R is a variance of a measurement sensor error.

The calibrating device may be configured to calibrate the state variable according to an equation of x_k={circumflex over (x)}_k+K_k(Z_k−H_k{circumflex over (x)}_k) and calibrate a variance of the state variable according to an equation of P_k=(I−K_kH_k){right arrow over (P)}_k(I−K_kH_k)′+K_kRK′_k, where K_k is a gain of the Kalman filter, Z_k is an output matrix of the Kalman filter, H_k is an observation matrix of the Kalman filter, I is a 3×3 unit matrix, and R is a variance of a measurement sensor error.

The quaternion calculating device may be configured to calculate the quaternion according to an equation of q_k={circumflex over (q)}_k⊗[1 q_ex q_ey q_ez]′_k after setting the calibrated state variable to be an error quaternion, where {circumflex over (q)}_k is an estimated quaternion and q_ex, q_ey, and q_ez are three-factor variables of the error quaternion.

The angular velocity calculator may be configured to arrange the differential expression according to factors and then discretize to acquire an angular velocity calculating equation represented by an equation of

$\left[\begin{array}{c}{\omega }_x^k\\ {\omega }_y^k\\ {\omega }_z^k\end{array}\right]=2/T\left[\begin{array}{c}{q}_0^k-q_0^{k-1}\\ q_x^k-q_x^{k-1}\\ q_y^k-q_y^{k-1}\\ q_z^k-q_z^{k-1}\end{array}\right]\left[\begin{array}{cccc}-q_x^k& q_0^k& q_z^k& q_y^k\\ -q_y^k& -q_z^k& q_0^k& q_x^k\\ -q_z^k& q_y^k& -q_x^k& q_0^k\end{array}\right]$

after acquiring a differential expression expressed by an equation of

$\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}}=\frac{1}{2}W\left(t\right)q\left(t\right)$

by differentiating the quaternion, W(t) is an angular velocity vector represented by an equation of W(t)=[0 ω_x ω_y ω_z]′, where q₀, q_x, q_y, and q_z are four factors which configure the quaternion, and T is a sampling time.

Other features and aspects will be apparent from the following detailed description, the drawings, and the claims.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a schematic view of an example of an angular velocity calculating apparatus.

FIG. 2 is a detailed diagram of an example of an angular velocity calculating device.

FIG. 3 is a flowchart of an example of an operation of an angular velocity calculating method using an acceleration sensor and a geomagnetic sensor.

FIG. 4 is a view illustrating a geomagnetic sensor and a coordinate system in the Northern hemisphere.

FIG. 5 is a view illustrating a coordinate system in which a geomagnetic sensor and an acceleration sensor simultaneously operate.

FIG. 6 is a view representing a relative angle change of a reference coordinate system and a user coordinate system.

FIG. 7 is a flowchart of another example of an operation of an angular velocity calculating method using an acceleration sensor and a geomagnetic sensor.

FIGS. 8 to 10 are views depicting an example of a performance of an angular velocity calculating method using an acceleration sensor and a geomagnetic sensor.

Throughout the drawings and the detailed description, the same reference numerals refer to the same elements. The drawings may not be to scale, and the relative size, proportions, and depiction of elements in the drawings may be exaggerated for clarity, illustration, and convenience.

DETAILED DESCRIPTION

The following detailed description is provided to assist the reader in gaining a comprehensive understanding of the methods, apparatuses, and/or systems described herein. However, various changes, modifications, and equivalents of the methods, apparatuses, and/or systems described herein will be apparent after an understanding of the disclosure of this application. For example, the sequences of operations described herein are merely examples, and are not limited to those set forth herein, but may be changed as will be apparent after an understanding of the disclosure of this application, with the exception of operations necessarily occurring in a certain order. Also, descriptions of features that are known in the art may be omitted for increased clarity and conciseness.

The features described herein may be embodied in different forms, and are not to be construed as being limited to the examples described herein. Rather, the examples described herein have been provided merely to illustrate some of the many possible ways of implementing the methods, apparatuses, and/or systems described herein that will be apparent after an understanding of the disclosure of this application.

According to the present disclosure, an angular velocity calculating method and apparatus using an acceleration sensor and a geomagnetic sensor calculate angular velocity using measurement values of the acceleration sensor and the geomagnetic sensor instead of using the gyro sensor.

FIG. 1 is a schematic view of an example of an angular velocity calculating apparatus and FIG. 2 is a detailed diagram of an example of an angular velocity calculating device according to the present disclosure.

Referring to FIGS. 1 and 2, the angular velocity calculating apparatus 100 of the present disclosure may include an acceleration sensor 110, a geomagnetic sensor 120, and an angular velocity calculating device 130.

The acceleration sensor 110 and the geomagnetic sensor 120 are sensors which measure and provide gravitational acceleration information and geomagnetic information.

However, the acceleration sensor 110 and the geomagnetic sensor 120 of the present disclosure provide information required to calculate angular velocity after a calibration process. If the acceleration sensor 110 includes a linear acceleration component in addition to the gravitational acceleration or the geomagnetic sensor 120 includes an offset, precision of an angular velocity calculation operation is significantly degraded.

The angular velocity calculating device 130 uses a Kalman filter to measure a relative angle variation between the user coordinate system and the reference coordinate system, which are used for measurements of the acceleration sensor 110 and the geomagnetic sensor 120, and calculates angular velocity ω therefrom. Specifically, according to the present disclosure, a method which sets and uses an error quaternion as a state variable is newly suggested to perform angular velocity calculating operations advantageous for linearization based thereon.

More specifically, the angular velocity calculating device 130 of the present disclosure may include an error quaternion setting device 131, an estimating device 132, a filter configuring device 133, a calibrating device 134, a quaternion calculating device 135, and an angular velocity calculator 136.

The error quaternion setting device 131 is configured to set an error quaternion. The estimating device 132 is configured to derive a state variable and a variance of the state variable based on the error quaternion, and convert a quaternion-based rotation matrix into an error quaternion-based rotation matrix. The filter configuring device 133 is configured to redefine an acceleration sensor output and an output of the geomagnetic sensor using the error quaternion-based rotation matrix to calculate an observation matrix and an output matrix of the Kalman filter, and transfer the state variable and the variance of the state variable through a discretized transfer matrix to calculate a gain of the Kalman filter. The calibrating device 134 is configured to calibrate the state variable and the variance of the state variable through the gain of the Kalman filter. The quaternion calculating device 135 is configured to calculate the quaternion-based on the calibrated state variable and the estimated quaternion. The angular velocity calculator 136 is configured to calculate angular velocity based on the quaternion.

If necessary, the angular velocity calculating device 130 of the present disclosure may further include an additional information acquirer 137 that calculates additional information such as an Euler angle, an Euler rotational axis and an angle, and linear acceleration, and a rotation matrix based on the quaternion.

As described above, the angular velocity calculating device 100 of the present disclosure does not use an actual gyro sensor, but uses the acceleration sensor 110 and the geomagnetic sensor 120 to calculate angular velocity that is generated by an actual gyro sensor, so as to avoid the problems or cons of a gyro sensor.

Moreover, the acceleration sensor 110 and the geomagnetic sensor 120 of the present disclosure may be sensors that are separately implemented for the angular velocity calculating apparatus 100, but may be a velocity sensor and an acceleration sensor which have been already provided in another device (for example, a portable terminal, a vehicle, a drone, and a game machine) in some cases.

That is, the angular velocity calculating apparatus of the present disclosure may be implemented in one independent hardware apparatus that includes the acceleration sensor 110, the geomagnetic sensor 120, and the angular velocity calculating device 130. However, in some cases, the angular velocity calculating apparatus may be implemented in an embedded form in which only the angular velocity calculating device 130 is mounted in another device or may be implemented in a software form in which only the angular velocity calculating device 130 is mounted in the other device or downloaded and provided in the other device.

FIG. 3 is a flowchart of an example of an operation of an angular velocity calculating method using an acceleration sensor and a geomagnetic sensor.

Referring to FIG. 3, the angular velocity calculating method of the present disclosure may include steps S1-S10. Step S1 derives a state variable and a variance of the state variable based on an error quaternion. Step S2 converts a quaternion-based rotation matrix into an error quaternion-based rotation matrix. Step S3 redefines an accelerometer output and an output of the geomagnetic sensor using the error quaternion-based rotation matrix. Step S4 calculates an observation matrix and an output matrix of the Kalman filter. Step S5 transfers the state variable and the variance of the state variable through a discretized transfer matrix. Step S6 calculates a gain of the Kalman filter based on the transferred variance of the state variable and the observation matrix of the Kalman filter. Step S7 calibrates the state variable and the variance of the state variable through the gain of the Kalman filter. Step S8 calculates a quaternion-based on the calibrated state variable and an estimated quaternion. Step S9 calculates angular velocity based on the quaternion. Step S10 inquires whether to request an end to the process. Step S11 re-enters step S1 to calculate angular velocity at a subsequent time when the angular velocity calculating process is still ongoing.

The angular velocity calculating method of the present disclosure may be described in more detail with reference to FIGS. 3 to 6.

The geomagnetic sensor 120 indicates a magnetic north direction in any circumstances to determine an absolute direction of the sensor.

FIG. 4 is a view illustrating the geomagnetic sensor 120 and a coordinate system in the Northern hemisphere. Referring to FIG. 4, it is understood that an inclination angle α is an angle with respect with a horizontal plane of the Earth when the geomagnetic sensor 120 is directed to the magnetic north. The inclination angle horizontal with the ground is “0” in the equator and approaches “90” degrees when it is closer to the North Pole. The inclination angle α is an angle affected by the position of the Earth but a component projected on the horizontal plane is directed to the magnetic north a direction that does not change. The inclination angle α does not change unless a disturbance exists. Geologically, the Earth's magnetic force line starts from the South Pole, that is, a magnetic south and is directed to the magnetic north. The Earth's magnetic force line is affected by a change of magnetism in the Earth and climate change. The International Association of Geomagnetism and Aeronomy announces a new geomagnetic standard which serves as a reference based on the Earth model every five years. The magnitude of the geomagnetic intensity on Earth varies depending on the position from approximately 25 to 65 μTesla (or 0.25 to 0.65 Gauss).

FIG. 5 is a view illustrating a coordinate system in which a geomagnetic sensor and an acceleration sensor simultaneously operate. Referring to FIG. 5, it is understood that the gravitational acceleration detected by the acceleration sensor 110 is directed to a vertical direction, the geomagnetism forms an inclination angle with the horizontal plane and is directed to the magnetic north. When there is no linear acceleration in the acceleration sensor 110, 9.8 m/sec² of the gravitational acceleration is detected in a −z direction.

The gravitational acceleration and geomagnetism are invariants that are detected in a reference coordinate system (reference frame or inertial frame X-Y-Z). However, the actual acceleration sensor 110 and geomagnetic sensor 120 acquire sensor output values in the user coordinate system (body frame X-Y-Z) so that when the user coordinate system rotates to match the coordinate systems, inversely, an angle of the user coordinate system may be calculated from the rotation angle.

The user coordinate system has moving and rotating characteristics, but only a change of the angle caused by the rotation is calculated in the present disclosure. ω is angular velocity when rotating around an arbitrary axis and causes a change of the angle with respect to the reference coordinate system.

That is, in the present disclosure, two vectors of the acceleration sensor and the geomagnetic sensor that are measured in the user coordinate system are ultimately converted into the reference coordinate system to calibrate the acceleration sensor and the geomagnetic sensor, and the angular velocity is calculated based on a relative angle change of the reference coordinate system and the user coordinate system which is generated at this time.

In the present disclosure, in order to represent the relative angle change of the reference coordinate system and the user coordinate system as illustrated in FIG. 6, a quaternion expression method as represented in Equation 1 or 2 may be introduced.

q=[q ₀ q ₁ q ₂ q ₃ ]′ϵR ⁴  Equation 1

q=[q ₀ q _x q _y q _z ]′ϵR ⁴Equation 2

In this case, q₀ is a factor corresponding to

$\cos \left(\frac{\theta }{2}\right)$

scalar in the quaternion. q₁, q₂, and q₃ or q_x, q_y, and q_z are sin(θ/2)i, sin(θ/2)j, sin(θ/2)k which correspond to vectors of x, y, and z axes, respectively. R⁴ expresses that the quaternion is a variable formed of four factors.

θ represents an Euler angle or a rotation angle according to a definition of Charles. i, j, and k are unit vectors of individual axes and the sign “′” is a transpose matrix.

An arbitrary point P_n in the space may be expressed by the reference coordinate system, P_b may be expressed by the user coordinate system and the correlation of P_n and P_b may be expressed by a rotation matrix C(q). This is represented by the quaternion to obtain Equation 3.

P _n =C(q)P_b  Equation 3

The rotation matrix C(q) is expressed by Equation 4. Next, when the quaternion is calculated and then converted into the rotation matrix, Equation 4 is utilized.

$\begin{array}{cc}C\left(q\right)=\left[\begin{array}{ccc}2q_0^2+2q_x^2-1& 2q_xq_y+2q_0q_z& 2q_xq_z-2q_0q_y\\ 2q_xq_y-2q_0q_z& 2q_0^2+2q_y^2-1& 2q_yq_z+2q_0q_x\\ 2q_xq_z+2q_0q_y& 2q_yq_z-2q_0q_x& 2q_0^2+2q_z^2-1\end{array}\right]& \mathrm{Equation}4\end{array}$

When the coordinate system rotates at specific angular velocity, a differential value of the quaternion may be expressed by an equation of motion of Equation 5 using the specific angular velocity.

{dot over (q)}=½q⊗ω  Equation 5

In this case, ⊗ is not a normal multiplication, but is a multiplication of quaternion which is different from the existing multiplication. The quaternion has four elements and the angular velocity has three elements so that different calculating methods are performed. Equation 5 is a formula used to calculate a quaternion at a subsequent time using a quaternion at a present time when the coordinate system rotates at the specific angular velocity ω. The formula of Equation 5 is used in a step of estimating a Kalman filter later.

The gravitational acceleration {right arrow over (G)} in the reference coordinate system measured in the acceleration sensor and the Earth geomagnetism M measured in the geomagnetic sensor may be expressed as vectors as represented in Equations 6 and 7.

$\begin{array}{cc}\overrightarrow{G}=\left[\begin{array}{c}0\\ 0\\ 9.8\end{array}\right]& \mathrm{Equation}6\\ \overrightarrow{M}=\left[\begin{array}{c}0\\ \cos \left(\alpha \right)\\ -\sin \left(\alpha \right)\end{array}\right]& \mathrm{Equation}7\end{array}$

In Equation 7, α is an inclination angle in FIG. 1. Specifically, the geomagnetism includes an intensity M₀ of the geomagnetism. The geomagnetic intensity varies according to the position of the user, so that the measured value is processed to be dimensionless to utilize only the angle information. The inclination angle is directed upwardly on the horizontal plane in the Southern hemisphere and a sign thereof is changed to “-”. Further, the reason why the gravitational acceleration is not processed to be dimensionless is that the gravitational acceleration is a constant number that does not vary according to the position so that the gravitational acceleration does not affect the expansion of the equation.

In the present disclosure, the gravitational acceleration vector and the geomagnetic vector, that is, two vectors serve as references to calculate a relative angle of an arbitrary vector in the space and to this end, the Kalman filter is applied.

Specifically, in the present disclosure, an indirect Kalman filter method that is configured by an error quaternion {right arrow over (q)}_e, which is more advantageous for linearization than the quaternion is utilized. The error quaternion is defined as represented in Equation 8 and is also utilized as a state of the state equation.

A physical amount which is estimated in the Kalman filter is a quaternion representing rotation information of the user coordinate system. Actually, the quaternion may be estimated based on a dynamic property of the system. The estimated quaternion {circumflex over (q)} may include an error. Due to an error caused by the mathematical model and an error generated when a nonlinear system is linearized, when the quaternion q is estimated, an error is inevitably included. This is an error which is inevitably generated even when the quaternion is estimated using the motion equation of Equation 5. The estimated quaternion including an error is expressed by {circumflex over (q)}. A calibration process to the Kalman filter is applied needs to be implemented and this process needs to be implemented using the acceleration and the geomagnetic value which are information ensured through the measurement. A quaternion which is used for quaternion calibration is a quaternion which represents a minute change amount, that is, an error quaternion ({right arrow over (q)}_e or q_e). The estimation and calibration of the Kalman filter are performed using Equations 5 and 8.

$\begin{array}{cc}q=\hat{q}\otimes q_e& \mathrm{Equation}8\\ q_e=\left[\begin{array}{c}1\\ {\overrightarrow{q}}_e\end{array}\right]& \mathrm{Equation}9\end{array}$

In this case, {right arrow over (q)}_e is [q_ex, q_ey, q_ez]′.

In the present disclosure, the acceleration sensor Y_a and the geomagnetic sensor Y_m, which are used to calculate the angular velocity ω using the Kalman filter, are defined by output equations (or output models) as represented in Equations 10 and 11.

Y _a =C(q){right arrow over (G)}+B_a+V_a  Equation 10

Y _m =C(q){right arrow over (M)}+V_m  Equation 11

In Equations 10 and 11, C(q) is a rotation matrix. In Equation 10, which is an output equation of the acceleration sensor, B_a is linear acceleration and is not represented in Equation 10 in a normal state (that is, when acceleration does not exist). V_a is a noise of the acceleration sensor and an average thereof is “0”. V_m is a noise of the geomagnetic sensor and an average thereof is also “0”. The variance varies depending on manufacturers of the acceleration sensor and the geomagnetic sensor so that a performance specification table is provided to the user. R_a is a variance of the acceleration sensor and R_m is a variance of the geomagnetic sensor.

E(V_aV′_a)=R_a  Equation 12

E(V_mV′_m)=R_mEquation 13

It is assumed that an offset of the acceleration sensor or the geomagnetic sensor is calibrated to be removed from the output equation of the sensor. In the geomagnetic sensor, the offset needs to be removed through a separate calibration process. When there is an offset, the offset causes an error in all processes that will be calculated thereafter. The offset is not a unique output value of the sensor but the offset is affected by its surrounding environment. Thus, when the sensor is used in a new environment, the offset needs to be recalibrated.

In order to configure the Kalman filter, as represented in Equation 5, the motion equation using the angular velocity ω is generally used. However, an object of the present disclosure is to functionally implement the gyro sensor so as to avoid using the gyro sensor, i.e., the functionality of a gyro sensor is implemented using an acceleration sensor and a geomagnetic sensor.

To this end, it is assumed that the current state and the previous state have a statistical correlation in which only time delay ti is generated. This may be assumed as a random process and it is assumed that a relationship between states is exponentially correlated. As a physical meaning, it means that a state at the present time and a state at a subsequent time are not significantly changed. This assumption is valid because a dynamic behavior of an operation of the user is significantly slower than a dynamic characteristic of the sensor so that factors, other than the motion of the user, are insignificant. Therefore, the dynamic characteristic of the user's motion when the user uses a mobile device such as a mobile phone does not affect the dynamic characteristics of the sensors. A state equation as represented in the following Equation 14 will be derived based on this assumption.

{dot over (x)}=Fx+Gw(t)  Equation 14

F and G are represented as follows.

$\begin{array}{cc}F=-\frac{1}{\tau }\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]& \mathrm{Equation}15\\ G=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]& \mathrm{Equation}16\end{array}$

In this case, τ is a time factor representing a correlated degree between states and may be represented by a constant. τ may have a value of 0.1 to 0.6 depending on an application system through experiment.

x is a state, F represents a transfer matrix, G represents a noise matrix, and w(t) represents a system noise. Equation 14 is a normal Kalman filter transfer equation so that in order to apply indirect Kalman filter, the state is set to be an error quaternion and is expressed by the following Equation 17. In Equation 17, the reason why only three elements corresponding to vector parts are selectively applied when the error quaternion is used as the state is that a part corresponding to the scalar is 1 which is a constant so that the part cannot be a variable.

$\begin{array}{cc}x=\left[\begin{array}{c}{q}_{\mathrm{ex}}\\ q_{\mathrm{ey}}\\ q_{\mathrm{ez}}\end{array}\right]& \mathrm{Equation}17\end{array}$

The current state is transferred to the subsequent state using Equation 14. Due to the property of the Kalman filter, the variance of the state variable needs to be transferred together, which will be expressed by the following Equation 18.

{circumflex over (P)}(t)=E{(x(t)−{circumflex over (x)}(t))(x(t)−{circumflex over (x)}(t))′}  Equation 18

In this case, x is a state variable, {circumflex over (x)} is an estimated state variable, and represents a transpose matrix.

The estimated state is calibrated using Equation 18 and the calibration uses the measurement value. When a system transfer matrix is regenerated based on the error quaternion, it may be expressed by Equation 19.

{dot over ({circumflex over (q)})}_e=F{circumflex over (q)}_e+Gw(t)  Equation 19

Equations 18 and 19 represent state estimating status, which are expressed by a symbol “A”. Further, the rotation matrix C(q) represented in the sensor output equations of Equations 10 and 11 is expressed by the quaternion q rather than the error quaternion q_e so that the rotation matrix needs to be expanded by an error quaternion suitable for Equation 19.

Therefore, when the rotation matrix is regenerated using Equation 8, the rotation matrix is expressed by a product of the estimated quaternion-based rotation matrix and the error quaternion-based rotation matrix and an error quaternion-based rotation matrix as represented in Equation 21 may be obtained based thereon.

$\begin{array}{cc}C\left(q\right)=C\left(q_e\right)C\left(\hat{q}\right)& \mathrm{Equation}20\\ C\left(q_e\right)=\left[\begin{array}{ccc}1+2q_{\mathrm{ex}}^2& 2q_{\mathrm{ex}}q_{\mathrm{ey}}+2q_{\mathrm{ez}}& 2q_{\mathrm{ex}}q_{\mathrm{ez}}-2q_{\mathrm{ey}}\\ 2q_{\mathrm{ex}}q_{\mathrm{ey}}-2q_{\mathrm{ez}}& 1+2q_{\mathrm{ey}}^2& 2q_{\mathrm{ey}}q_{\mathrm{ez}}+2q_{\mathrm{ex}}\\ 2q_{\mathrm{ex}}q_{\mathrm{ez}}+2q_{\mathrm{ey}}& 2q_{\mathrm{ey}}q_{\mathrm{ez}}-2q_{\mathrm{ex}}& 1+2q_{\mathrm{ez}}^2\end{array}\right]& \mathrm{Equation}21\end{array}$

In this case, under an assumption that q_ex, q_ey, q_ez are minute amounts and products those are more minute amounts, terms belonging to Equation 22 may be removed so that Equation 21 may be simplified as represented in Equation 23.

$\begin{array}{cc}{q}_{\mathrm{ex}}q_{\mathrm{ey}}=q_{\mathrm{ey}}q_{\mathrm{ez}}=q_{\mathrm{ex}}q_{\mathrm{ez}}=q_{\mathrm{ex}}^2=q_{\mathrm{ey}}^2=q_{\mathrm{ez}}^2\approx 0& \mathrm{Equation}22\\ C\left(q_e\right)\approx \left[\begin{array}{ccc}1& 2q_{\mathrm{ez}}& -2q_{\mathrm{ey}}\\ -2q_{\mathrm{ez}}& 1& 2q_{\mathrm{ex}}\\ 2q_{\mathrm{ey}}& -2q_{\mathrm{ex}}& 1\end{array}\right]=I_{3\times 3}-2q_e\times & \mathrm{Equation}23\end{array}$

In this case, q_ex represents

$\hspace{1em}\left[\begin{array}{ccc}0& -q_{\mathrm{ez}}& q_{\mathrm{ey}}\\ q_{\mathrm{ez}}& 0& -q_{\mathrm{ex}}\\ -q_{\mathrm{ey}}& -2q_{\mathrm{ex}}& 0\end{array}\right]$

and is a sign simply expressing a process of multiplying an arbitrary 3×1 matrix or 3×3 matrix.

Further, C({circumflex over (q)}) in Equation 20 is an estimated quaternion-based rotation matrix and is represented as in Equation 23.

C(q)=C(q_e)C({circumflex over (q)})=(I−2q_ex)C({circumflex over (q)})  Equation 24

Finally, since the rotation matrix is expressed based on the error quaternion q_e, the rotation matrix may be substituted into the state equation of Equation 19. The sensor output equation may be re-represented using Equation 24 as follows.

Y _a −C({circumflex over (q)}){right arrow over (G)}=2C({circumflex over (q)}){right arrow over (G)}q_e+B_a+V_a  Equation 25

Y _m =C({circumflex over (q)}){right arrow over (M)}=2C({circumflex over (q)}){right arrow over (M)}q_e+V_m  Equation 26

An observation matrix of the Kalman filter is represented based on Equations 25 and 26 as follows.

$\begin{array}{cc}H=\left[\begin{array}{c}2C\left(\hat{q}\right)\overrightarrow{G}q_e\\ 2C\left(\hat{q}\right)\overrightarrow{M}q_e\end{array}\right]& \mathrm{Equation}27\end{array}$

The output matrix of the Kalman filter is expressed by Equation 28.

$\begin{array}{cc}Z=\left[\begin{array}{c}{Y}_a-C\left(\hat{q}\right)\overrightarrow{G}\\ Y_m-C\left(\hat{q}\right)\overrightarrow{M}\end{array}\right]& \mathrm{Equation}28\end{array}$

In order to apply the present disclosure to an actual product, a discretization process is necessary. When the transfer matrix is discretized, a sampling time is expressed by T. Since the error quaternion (that is, the state variable and variance of the state variable) is transferred using the discretized transfer matrix, transfer equations of the state variable and the variance of the state variable are expressed by Equations 29 and 30.

For reference, in Equations 14 and 19, Gw(t) is included. However, Equation 29 is an equation for a process of transferring an estimated value and w(t) is a noise whose average is “0”, so that in Equation 29 which is expanded by estimated value, Gw(t) is erased.

{circumflex over (x)} _k+1 =F _k {circumflex over (x)} _k  Equation 29

{circumflex over (P)} _k+1 =F _k {circumflex over (P)} _k F′ _k +Qd _k  Equation 30

In this case, F_k is a discretized transfer matrix and T is a sampling time.

$\begin{array}{cc}{F}_k=e^{F*T}=e^{-\frac{T}{\tau }\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]}& \mathrm{Equation}31\end{array}$

Further, Q_dk is a discretized process noise variance of Q so that it is expressed as in Equation 32. When Equations 31 and 32 are more simplified under an assumption that an equation including an integral element is a first-order linear equation, Equations 31 and 32 are expressed by Equations 33 and 34.

$\begin{array}{cc}{\mathrm{Qd}}_k={\int }_k^{k+1}e^{F*T}Qe^{F*T^{\prime }}\mathrm{dt}& \mathrm{Equation}32\\ F_k\approx I+F\left(\mathrm{kT}\right)T+1/2{F\left(\mathrm{kT}\right)}^2T^2& \mathrm{Equation}33\\ {\mathrm{Qd}}_k\approx \mathrm{QT}+1/2F\left(\mathrm{kT}\right)+1/2{F\left(\mathrm{kT}\right)}^{\prime }& \mathrm{Equation}34\end{array}$

When a gain of the Kalman filter is calculated using the above-described equations, the gain is expressed by Equation 35.

K _k =P _k H′ _k(H_k{circumflex over (P)}_kH′_k+R)⁻¹  Equation 35

In Equation 35, the matrix R is a measurement matrix variance of the sensor or a variance of a measurement sensor error and may be defined by the following Equation 36 using the above-described Equations 12 and 13.

$\begin{array}{cc}R=\left[\begin{array}{cc}{R}_aI_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& R_mI_{3\times 3}\end{array}\right]& \mathrm{Equation}36\end{array}$

In this case, I_3×3 is a 3×3 unit matrix and 0_3×3 is a 3×3 0 matrix.

When the state variable and the variance of the state variable are calibrated using the gain of the Kalman filter, the calibrated state variable and variance of the state variable are expressed by Equations 37 and 38.

x _k ={circumflex over (x)} _k +K _k(Z_k−H_k{circumflex over (x)}_k)  Equation 37

P _k=(I−K_kH_k){circumflex over (P)}_k(I−K_kH_k)′+K_kRK′_k  Equation 38

It is confirmed that when the calibration is performed in Equations 37 and 38, the state variable and the variance of the state variable are not estimated variables, so that “̂” disappears.

The calibrated state variable x_k is an error quaternion q_e and as represented in Equation 39, the error quaternion q_e and the estimated quaternion q are multiplied to calculate the quaternion q. In this case, the quaternion q is a variable having four factors and the error quaternion q_e is a variable having three factors, so that a constant of 1 is added to a factor corresponding to scalar in the error quaternion q_e.

q _k ={circumflex over (q)} _k⊗[1 q_ex q_ey q_ez]′_k  Equation 39

The process of calculating the angular velocity {right arrow over (ω)}=ω_xi+ω_yj+ω_zk in the quaternion calculated as described above is as follows.

First, when the quaternion is differentiated, it is expressed by the following Equation 40.

$\begin{array}{cc}\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}}=\frac{1}{2}W\left(t\right)q\left(t\right)& \mathrm{Equation}40\end{array}$

In this case, W(t) represents an angular velocity vector as represented in Equation 5, but the quaternion is a variable having four factors and {right arrow over (ω)} is a variable having three factors, so that W(t) is a variable which is adjusted to be W(t)=[0 ω_x ω_y ω_z]′ for calculation.

Equation 40 is represented for every factor as follows.

$\begin{array}{cc}\frac{{\mathrm{dq}}_0}{\mathrm{dt}}=-\frac{1}{2}\left({\omega }_xq_x+{\omega }_yq_y+{\omega }_zq_z\right)\frac{{\mathrm{dq}}_x}{\mathrm{dt}}=\frac{1}{2}\left({\omega }_xq_0+{\omega }_yq_z+{\omega }_zq_y\right)\frac{{\mathrm{dq}}_y}{\mathrm{dt}}=\frac{1}{2}\left({\omega }_yq_0+{\omega }_zq_x+{\omega }_zq_z\right)\frac{{\mathrm{dq}}_z}{\mathrm{dt}}=-\frac{1}{2}\left({\omega }_zq_0+{\omega }_xq_y+{\omega }_zq_z\right)& \mathrm{Equation}41\end{array}$

Equation 41 may be represented to be Equation 42.

$\begin{array}{cc}W\left(t\right)=2\frac{\mathrm{dq}\left(t\right)}{\mathrm{dt}}{q\left(t\right)}^{-1}& \mathrm{Equation}42\end{array}$

In this case, q⁻¹ is q⁻¹=[q₀ −q_x −q_y −q_z]′ according to the definition.

Equations 41 and 42 are discretized to calculate ω_x^k, ω_y^k, ω_z^k as represented in Equation 43. In this case, an index k−1 represents a previous step of the discretized variable.

$\begin{array}{cc}\left[\begin{array}{c}{\omega }_x^k\\ {\omega }_y^k\\ {\omega }_z^k\end{array}\right]=2\text{/}T\left[\begin{array}{c}{q}_0^k-q_0^{k-1}\\ q_x^k-q_x^{k-1}\\ q_y^k-q_y^{k-1}\\ q_z^k-q_z^{k-1}\end{array}\right]\left[\begin{array}{cccc}-q_x^k& q_0^k& q_z^k& q_y^k\\ -q_y^k& -q_z^k& q_0^k& q_x^k\\ -q_z^k& q_y^k& -q_x^k& q_0^k\end{array}\right]& \mathrm{Equation}43\end{array}$

By doing this, an angular velocity which is typically measured by a gyro sensor may be calculated using the acceleration sensor and the geomagnetic sensor based on Equation 43. Thus, the functionality of a gyro sensor is implemented using an acceleration sensor and a geomagnetic sensor, i.e., an angular velocity of an object or device is determined without a gyro sensor using an acceleration sensor and a geomagnetic sensor.

FIG. 7 is a flowchart of another example of an operation of an angular velocity calculating method using an acceleration sensor and a geomagnetic sensor.

As illustrated in FIG. 7, an angular velocity calculating method according to another example of the present disclosure may further include at least one of a step S21 of calculating an Euler angle using the above-described quaternion, a step S22 of calculating Euler rotational axis and angle using the quaternion, a step S23 of calculating linear acceleration using the quaternion, and a step S24 of calculating a rotation matrix using the quaternion.

The above-described quaternion may be converted into various types of outputs.

First, the Euler angle may be expressed using the quaternion as represented in following Equations 44, 45, and 46. The Euler angles are represented to be different according to an order of rotating an axis so that 12 combinations of Euler angles may be expressed. In the present disclosure, as a representative example, the axes rotate in the order of a z axis, a y axis, and an x axis (3-2-1).

Roll=tan⁻¹(2(q₀q_x+q_yq_z),1−2(q_x²+q_y²))  Equation 44

Pitch=sin⁻¹(2(q₀q_y−q_zq_x))  Equation 45

Yaw=tan⁻¹(2(q₀q_z+q_xq_y),1−2(q_y²+q_z²))  Equation 46

Further, the Euler axis ê and the Euler angle ω_e may be expressed using the quaternion by Equations 47 and 48.

$\begin{array}{cc}\hat{e}=\frac{\overrightarrow{q}}{\overrightarrow{q}}& \mathrm{Equation}47\\ {\theta }_e=2{\cos }^{-1}\left(q_0\right)& \mathrm{Equation}48\end{array}$

In this case, {right arrow over (q)}=q_xi+q_yj+q_zk is an element corresponding to a vector of the quaternion, i, j, and k are unit vectors in each axis, and ∥{right arrow over (q)}∥ represents an absolute value of the vector {right arrow over (q)}.

In Equation 10, Y_a is an output value measured by the acceleration sensor and {right arrow over (G)} is gravitational acceleration, so that linear acceleration of an object which is in motion may also be calculated using the quaternion as represented in Equation 49.

B _a =−Y _a +C(q){right arrow over (G)}  Equation 49

Further, the above-described rotation matrix as represented in Equation 4 may also be calculated using the quaternion.

FIGS. 8 to 10 are views depicting an example of a performance of an angular velocity calculating method based on an acceleration sensor and a geomagnetic sensor.

FIG. 8 is a view for comparing actual measurement values of an acceleration sensor and a geomagnetic sensor when the geomagnetic sensor is calibrated and an azimuth is measured. It is understood that when the geomagnetic sensor is calibrated and an azimuth is measured, the acceleration sensor and the geomagnetic sensor horizontally operate, so that slight rotation acceleration due to a rotational motion is detected from a signal of the acceleration sensor but linear acceleration is not detected.

FIG. 9 is a view for comparing an actual measurement value of the gyro sensor and a calculated value of angular velocity according to the present disclosure.

Referring to FIG. 9, it is observed that in an output of the gyro sensor, a rotational motion is measured only on the z axis. However, the calculated value of the angular velocity includes noise signal not only on the z axis, but also on the x axis and the y axis. This is because a noise generated during the process of differentiating the quaternion is mixed. The shorter the sampling time, the larger the noise. However, the noise is a random noise so that the average thereof is “0”. Therefore, the noise may be easily removed by applying an average filter and saturation phenomenon which is generated as time passes by may be avoided.

FIG. 10 is a view for comparing a value of a z axis of the gyro sensor and a value of a z axis of the calculated angular velocity. Referring to FIG. 10, it is confirmed that even though a time delay and a noise are observed from the signal of the z axis of the calculated angular velocity, the signal faithfully follows a basic form of the z axis value of the gyro sensor. The noise may be removed using the average filter and a degree of time delay may be tuned by adjusting τ of Equation 15.

Therefore, according to the present disclosure, the need for an actual gyro sensor is obviated, which prevents various problems due to the gyro sensor, thereby reducing cost, manufacturing complexity, power consumption, and the size of the device that previously included the actual gyro sensor.

The acceleration sensor 110, geomagnetic sensor 120, angular velocity calculating device 130, error quaternion setting device 131, estimating device 132, filter configuring device 133, calibrating device 134, quaternion calculating device 135, angular velocity calculator 136, and additional information acquirer 137, in FIG. 1-10 that perform the operations described in this application are implemented by hardware components configured to perform the operations described in this application that are performed by the hardware components. Examples of hardware components that may be used to perform the operations described in this application where appropriate include controllers, sensors, generators, drivers, memories, comparators, arithmetic logic units, adders, subtractors, multipliers, dividers, integrators, and any other electronic components configured to perform the operations described in this application. In other examples, one or more of the hardware components that perform the operations described in this application are implemented by computing hardware, for example, by one or more processors or computers. A processor or computer may be implemented by one or more processing elements, such as an array of logic gates, a controller and an arithmetic logic unit, a digital signal processor, a microcomputer, a programmable logic controller, a field-programmable gate array, a programmable logic array, a microprocessor, or any other device or combination of devices that is configured to respond to and execute instructions in a defined manner to achieve a desired result. In one example, a processor or computer includes, or is connected to, one or more memories storing instructions or software that are executed by the processor or computer. Hardware components implemented by a processor or computer may execute instructions or software, such as an operating system (OS) and one or more software applications that run on the OS, to perform the operations described in this application. The hardware components may also access, manipulate, process, create, and store data in response to execution of the instructions or software. For simplicity, the singular term “processor” or “computer” may be used in the description of the examples described in this application, but in other examples multiple processors or computers may be used, or a processor or computer may include multiple processing elements, or multiple types of processing elements, or both. For example, a single hardware component or two or more hardware components may be implemented by a single processor, or two or more processors, or a processor and a controller. One or more hardware components may be implemented by one or more processors, or a processor and a controller, and one or more other hardware components may be implemented by one or more other processors, or another processor and another controller. One or more processors, or a processor and a controller, may implement a single hardware component, or two or more hardware components. A hardware component may have any one or more of different processing configurations, examples of which include a single processor, independent processors, parallel processors, single-instruction single-data (SISD) multiprocessing, single-instruction multiple-data (SIMD) multiprocessing, multiple-instruction single-data (MISD) multiprocessing, and multiple-instruction multiple-data (MIMD) multiprocessing.

The methods illustrated in FIGS. 3-7 that perform the operations described in this application are performed by computing hardware, for example, by one or more processors or computers, implemented as described above executing instructions or software to perform the operations described in this application that are performed by the methods. For example, a single operation or two or more operations may be performed by a single processor, or two or more processors, or a processor and a controller. One or more operations may be performed by one or more processors, or a processor and a controller, and one or more other operations may be performed by one or more other processors, or another processor and another controller. One or more processors, or a processor and a controller, may perform a single operation, or two or more operations.

Instructions or software to control computing hardware, for example, one or more processors or computers, to implement the hardware components and perform the methods as described above may be written as computer programs, code segments, instructions or any combination thereof, for individually or collectively instructing or configuring the one or more processors or computers to operate as a machine or special-purpose computer to perform the operations that are performed by the hardware components and the methods as described above. In one example, the instructions or software include machine code that is directly executed by the one or more processors or computers, such as machine code produced by a compiler. In another example, the instructions or software includes higher-level code that is executed by the one or more processors or computer using an interpreter. The instructions or software may be written using any programming language based on the block diagrams and the flow charts illustrated in the drawings and the corresponding descriptions in the specification, which disclose algorithms for performing the operations that are performed by the hardware components and the methods as described above.

The instructions or software to control computing hardware, for example, one or more processors or computers, to implement the hardware components and perform the methods as described above, and any associated data, data files, and data structures, may be recorded, stored, or fixed in or on one or more non-transitory computer-readable storage media. Examples of a non-transitory computer-readable storage medium include read-only memory (ROM), random-access memory (RAM), flash memory, CD-ROMs, CD-Rs, CD+Rs, CD-RWs, CD+RWs, DVD-ROMs, DVD-Rs, DVD+Rs, DVD-RWs, DVD+RWs, DVD-RAMs, BD-ROMs, BD-Rs, BD-R LTHs, BD-REs, magnetic tapes, floppy disks, magneto-optical data storage devices, optical data storage devices, hard disks, solid-state disks, and any other device that is configured to store the instructions or software and any associated data, data files, and data structures in a non-transitory manner and provide the instructions or software and any associated data, data files, and data structures to one or more processors or computers so that the one or more processors or computers can execute the instructions. In one example, the instructions or software and any associated data, data files, and data structures are distributed over network-coupled computer systems so that the instructions and software and any associated data, data files, and data structures are stored, accessed, and executed in a distributed fashion by the one or more processors or computers.

While this disclosure includes specific examples, it will be apparent after an understanding of the disclosure of this application that various changes in form and details may be made in these examples without departing from the spirit and scope of the claims and their equivalents. The examples described herein are to be considered in a descriptive sense only, and not for purposes of limitation. Descriptions of features or aspects in each example are to be considered as being applicable to similar features or aspects in other examples. Suitable results may be achieved if the described techniques are performed in a different order, and/or if components in a described system, architecture, device, or circuit are combined in a different manner, and/or replaced or supplemented by other components or their equivalents. Therefore, the scope of the disclosure is defined not by the detailed description, but by the claims and their equivalents, and all variations within the scope of the claims and their equivalents are to be construed as being included in the disclosure.

Claims

1. A processor-implemented method based on an acceleration sensor and a geomagnetic sensor for determining an angular velocity of an object, comprising: deriving a state variable and a variance of the state variable based on an error quaternion and converting a quaternion-based rotation matrix into an error quaternion-based rotation matrix;calculating an observation matrix and an output matrix of a Kalman filter after redefining an output of the acceleration sensor and an output of the geomagnetic sensor using the error quaternion-based rotation matrix;calculating a gain of the Kalman filter based on the transferred variance of the state variable and the observation matrix of the Kalman filter after transferring the state variable and the variance of the state variable through a discretized transfer matrix;calculating a quaternion based on a calibrated state variable and an estimated quaternion after calibrating the state variable and the variance of the state variable through the gain of the Kalman filter; andcalculating the angular velocity based on the quaternion.

2. The method of claim 1, wherein in the converting of the quaternion-based rotation matrix into the error quaternion-based rotation matrix, the error quaternion [qex, qey, qez]′ of the quaternion q=[q0, qx, qy, qz]′, which expresses a relative angle change between a reference coordinate system and a sensor coordinate system, is set to a state variable x.

3. The method of claim 2, wherein in the converting of the quaternion-based rotation matrix into the error quaternion-based rotation matrix, when a current state and a previous state have a statistic correlation in which only time delay ti is generated, the state variable x is expressed by an equation of {dot over (x)}=Fx, is a transfer matrix expressed by

4. The method of claim 1, wherein in the converting of the quaternion-based rotation matrix into the error quaternion-based rotation matrix, a variance of the state variable is calculated by an equation of {circumflex over (P)}(t)=E{(x(t)−{circumflex over (x)}(t))(x(t)−{circumflex over (x)}(t))′}, where x is a state variable, {circumflex over (x)} is an estimated state variable, and is a transpose matrix.

5. The method of claim 1, wherein in the converting of the quaternion-based rotation matrix into the error quaternion-based rotation matrix, the quaternion-based rotation matrix is converted into the error quaternion-based rotation matrix based on an equation of C(q)=C(qe)C({circumflex over (q)}), the quaternion-based rotation matrix is

6. The method of claim 5, wherein in the converting of the quaternion-based rotation matrix into the error quaternion-based rotation matrix, when qex, qey, and qez are minute amounts, the error quaternion-based rotation matrix expressed by

7. The method of claim 1, wherein in the calculating of the observation matrix and the output matrix of the Kalman filter, an output Ya of acceleration sensor and an output Ym of the geomagnetic sensor are redefined based on a correlation between the error quaternion qe expressed by C(q)=C(qe)C({circumflex over (q)})=(I−2qex)C({right arrow over (q)}) and the quaternion-based rotation matrix C(q), the acceleration sensor output Ya is Ya−C({circumflex over (q)}){right arrow over (G)}=2C({circumflex over (q)}){right arrow over (G)}qe+Ba+Va, the output Ym of the geomagnetic sensor is Ym−C({circumflex over (q)}){right arrow over (M)}=2C({circumflex over (q)}){right arrow over (M)}qe+Vm, where C({circumflex over (q)}) is an estimated quaternion-based rotation matrix, I3×3 is a 3×3 unit matrix, {right arrow over (G)} is a gravitational acceleration vector, Ba is linear acceleration, Va is a noise of the acceleration sensor, {right arrow over (M)} is an earth geomagnetic vector, and Vm is a noise of the geomagnetic sensor.

8. The method of claim 7, wherein in the calculating of an observation matrix and an output matrix of the Kalman filter, based on the acceleration sensor output Ya and the output Ym of the geomagnetic sensor, an observation matrix of the Kalman filter of

9. The method of claim 1, wherein in the calculating of the gain of the Kalman filter, the state variable x is transferred according to an equation of {circumflex over (x)}k+1=Fk{circumflex over (x)}k and a variance p of the state variable is transferred according to an equation of {circumflex over (P)}k+1=Fk{circumflex over (P)}kF′k+Qdk, where Fk is a discretized transfer matrix expressed by an equation of Fk≈I+F(kT)T+½F(kT)2T2, Qdk is a discretized process noise variance of Q expressed by an equation of Qdk≈QT+½F(kT)+½F(kT)′, and T is a sampling time.

10. The method of claim 9, wherein in the calculating of the gain of the Kalman filter, the discretized transfer matrix expressed by an equation of

11. The method of claim 9, wherein in the calculating of the gain of the Kalman filter, the gain of the Kalman filter is calculated according to an equation of Kk=PkH′k(Hk{circumflex over (P)}kH′k+R)−1, where R is a variance of a measurement sensor error and is expressed by an equation of

12. The method of claim 1, wherein in the calculating of the quaternion, the state variable is calibrated according to an equation of xk={circumflex over (x)}k+Kk(Zk−Hk{circumflex over (x)}k) and a variance of the state variable is calibrated according to an equation of Pk=(I−KkHk){circumflex over (P)}k(I−KkHk)′+KkRK′k, where Kk is a gain of the Kalman filter, Zk is an output matrix of the Kalman filter, Hk is the observation matrix of the Kalman filter, I is a 3×3 unit matrix, and R is a variance of a measurement sensor error.

13. The method of claim 12, wherein in the calculating of the quaternion, after setting the calibrated state variable to be the error quaternion, the quaternion is calculated according to an equation of qk={circumflex over (q)}k⊗[1 qex qey qez]′k, where {circumflex over (q)}k is an estimated quaternion and qex, qey, and qez are three-factor variables of the error quaternion.

14. The method of claim 1, wherein in the calculating of angular velocity based on the quaternion, after acquiring a differential expression expressed by an equation of

15. The method of claim 14, wherein in the calculating of angular velocity based on the quaternion, the differential expression is arranged for every factor to derive and arrange equations of

16. The method of claim 1, further comprising: calculating an Euler angle by rotating the Euler angle in the order of a z axis, a y axis, and an x axis in the quaternion,wherein the Euler angle is calculated based on at least one equation of Roll=tan−1(2(q0qx+qyqz),1−2(qx2+qy2)), Pitch=sin−1(2(q0qy−qzqx)), and Yaw=tan−1(2(q0qz+qxqy),1−2 (qy2+qz2)), and q0, qx, qy, and qz are four factors which configure the quaternion.

17. The method of claim 1, further comprising: calculating an Euler rotational axis and angle based on the quaternion,wherein the Euler rotational axis is calculated according to an equation of

18. The method of claim 1, further comprising: calculating linear acceleration of an object which is in motion based on the quaternion,wherein the linear acceleration is calculated according to an equation of Ba=−Ya C(q){right arrow over (G)}, where Ya is an acceleration sensor output, C(q) is a quaternion, and {right arrow over (G)} is a gravitational acceleration vector.

19. An angular velocity calculating apparatus based on an acceleration sensor and a geomagnetic sensor for determining an angular velocity of an object, comprising: an error quaternion setting device configured to set an error quaternion;an estimating device configured to derive a state variable and a variance of the state variable based on the error quaternion, and convert a quaternion-based rotation matrix into an error quaternion-based rotation matrix;a filter configuring device configured to redefine an acceleration sensor output and an output of the geomagnetic sensor using the error quaternion-based rotation matrix to calculate an observation matrix and an output matrix of a Kalman filter, and transfer the state variable and the variance of the state variable through a discretized transfer matrix to calculate a gain of the Kalman filter;a calibrating device configured to calibrate the state variable and the variance of the state variable through the gain of the Kalman filter;a quaternion calculating device configured to calculate a quaternion based on the calibrated state variable and the estimated quaternion; andan angular velocity calculator configured to calculate the angular velocity based on the quaternion.

20. The angular velocity calculating apparatus of claim 19, wherein the estimating device is configured to set an error quaternion [qex, qey, qez]′ of a quaternion q=[q0, qx, qy, qz]′ which expresses a relative angle change between a reference coordinate system and a sensor coordinate system to a state variable (x), when a current state and a previous state have a statistic correlation in which only time delay ti is generated, the state variable x is expressed by an equation of {dot over (x)}=Fx, where F is a transfer matrix expressed by

21. The angular velocity calculating apparatus of claim 19, wherein the estimating device is configured to convert the quaternion-based rotation matrix into the error quaternion-based rotation matrix based on an equation of C(q)=C(qe)C({circumflex over (q)}), the quaternion-based rotation matrix is

22. The angular velocity calculating apparatus of claim 19, wherein the filter configuring device is configured to redefine an output Ya of the acceleration sensor and an output Ym of the geomagnetic sensor based on a correlation between the error quaternion qe expressed by C(q)=C(qe)C({circumflex over (q)})=(I−2qex)C({circumflex over (q)}) and the quaternion-based rotation matrix C(q), the redefined acceleration sensor output Ya is Ya−C({circumflex over (q)}){right arrow over (G)}=2C({circumflex over (q)}){right arrow over (G)}qe+Ba+Va, the redefined output Ym of the geomagnetic sensor is Ym−C({circumflex over (q)}){right arrow over (M)}=2C({circumflex over (q)}){right arrow over (M)}qe+Vm, where C({circumflex over (q)}) is an estimated quaternion-based rotation matrix, I3×3 is a 3×3 unit matrix, {right arrow over (G)} is a gravitational acceleration vector, Ba is linear acceleration, Va is a noise of the acceleration sensor, {right arrow over (M)} is an earth geomagnetic vector, and Vm is a noise of the geomagnetic sensor.

23. The angular velocity calculating apparatus of claim 22, wherein the filter configuring device is configured to calculate an observation matrix of the Kalman filter of

24. The angular velocity calculating apparatus of claim 19, wherein the filter configuring device is configured to transfer the state variable (x) according to an equation of {circumflex over (x)}k+1=Fk{circumflex over (x)}k, transfer a variance p of the state variable according to an equation of {circumflex over (P)}k+1=Fk{circumflex over (P)}kF′k+Qdk, and then calculate a gain of the Kalman filter according to an equation of Kk=PkH′k(Hk{circumflex over (P)}kH′k+R)−1, where Fk is a discretized transfer matrix represented by an equation of Fk≈I+F(kT)T+½F(kT)T+½F(kT)2T2, Qdk is a discretized process noise variance of Q represented by an equation of Qdk≈QT+½F(kT)+½F(kT)′, T is a sampling time, Hk is an observation matrix of the Kalman filter, and R is a variance of a measurement sensor error.

25. The angular velocity calculating apparatus of claim 19, wherein the calibrating device is configured to calibrate the state variable according to an equation of xk={circumflex over (x)}k+Kk(Zk−Hk{circumflex over (x)}k) and calibrate a variance of the state variable according to an equation of Pk=(I−KkHk){circumflex over (P)}k(I−KkHk)′+KkRK′k, where Kk is a gain of the Kalman filter, Zk is an output matrix of the Kalman filter, Hk is an observation matrix of the Kalman filter, I is a 3×3 unit matrix, and R is a variance of a measurement sensor error.

26. The angular velocity calculating apparatus of claim 19, wherein the quaternion calculating device is configured to calculate the quaternion according to an equation of qk={circumflex over (q)}k⊗[1 qex qey qez′]k after setting the calibrated state variable to be an error quaternion, where {circumflex over (q)}k is an estimated quaternion and qex, qey, and qez are three-factor variables of the error quaternion.

27. The angular velocity calculating apparatus of claim 19, wherein the angular velocity calculator is configured to arrange a differential expression according to factors and then discretizes to acquire an angular velocity calculating equation represented by an equation of