Contents lists available atScienceDirect
Sensors and Actuators A: Physical
j o u r n a l h o m e p a g e : w w w . e l s e v i e r . c o m / l o c a t e / s n aDesign and analysis of an orientation estimation system using coplanar gyro-free
inertial measurement unit and magnetic sensors
C.F. Kao, T.L. Chen
∗Department of Mechanical Engineering, National Chiao Tung University, Hsinchu 30056, Taiwan, ROC
a r t i c l e i n f o
Article history:Received 5 May 2007
Received in revised form 25 October 2007 Accepted 12 February 2008
Available online 10 March 2008 Keywords:
Accelerometers Gyroscopes
Gyro-free inertial measurement unit Euler angles
Sensor fusion System observability
a b s t r a c t
Conventionally, there are two ways to determine the orientation angles of an object. One uses 3-axis accelerometers and 3-axis magnetic sensors, while the other one uses additional 3-axis gyroscopes to achieve a higher sensing accuracy than the first one. This paper presents a novel orientation estimation system that uses seven single-axis linear accelerometers and 3-axis magnetic sensors. These seven linear accelerometers are deployed in a way to form a “coplanar” gyro-free inertial measurement unit (IMU). As such, they can perform the integrated measurements of gravity force and angular velocity for the subsequent sensor fusion algorithms. With the information of the angular velocity, this novel design achieves a comparable performance with the one that includes 3-axis gyroscopes. The analysis of the system observability indicates that the angular velocity measurements can improve the observability of the angle estimation under certain circumstances. This result suggests the possibility of using fewer sensors in an orientation estimation system. Simulation results indicate that the proposed design achieve a sensing accuracy of 0.11◦, 0.09◦, and 0.2◦for the roll, pitch, and yaw angle; this leads to the improvement of 540%, 632%, and 754% over the one that does not include the measurement of the angular velocity.
© 2008 Elsevier B.V. All rights reserved.
1. Introduction
At a location around the earth surface, the intensity of earth magnetic field is about 0.5–0.6 G and has a component parallel to the earth’s surface that always points toward magnetic north[1]. Meanwhile, the earth gravity force is roughly at 9.8 m/s2and always points to the center of the earth. Therefore, for the accelerometers and magnetic sensors that attached to an object and rotated with the object, these sensors can detect portion of the gravity force and earth magnetic field depending on the orientation of the object. Reversely, these sensor measurements can be processed to determine the orientation of an object.
Using 3-axis linear accelerometers and 3-axis magnetic sensors is one of the most popular methods to determine the orientation angles of an object. However, in practice, the accelerometer measurements are often contaminated by the sensor noise and drift; the magnetic sensor measurements are often interrupted by the surroundings. This leads to the inaccuracy of the angle determination. Some researchers included 3-axis rate gyroscopes in an orientation estimation system[2–4]. In that approach, the angular velocity measurements, obtained from gyroscopes, and the angle measurements, obtained from accelerometers and magnetic sensors, are integrated by the “sensor fusion” techniques to improve the accuracy of the angle estimation. Most of these research reports focus on the relations between the angle estimation accuracy and sensor fusion algorithms. We did not find many discussions on the issue of employing different sensor components in an orientation estimation system.
Generally speaking, there are two ways to measure the 3-axis angular velocity using inertial sensors. One uses three gyroscopes, while the other one uses linear accelerometers together with signal processing techniques. Therefore, the second approach is often referred to as a gyro-free inertial measurement unit (IMU)[5,6]. When operating as a stand-alone angular velocity sensor, a gyroscope offers a higher sensing accuracy than a gyro-free IMU due to its high sensitivity and no mathematical integral operation required[7]. On the other hand, when operating as one of the sensor components in an orientation estimation system, the performance difference between gyroscopes and gyro-free IMUs is reduced because they only play portion of roles in improving the accuracy of the angle estimation. Therefore, it is
∗ Corresponding author.
E-mail addresses:ks07042@yahoo.com.tw(C.F. Kao),tsunglin@mail.nctu.edu.tw(T.L. Chen). 0924-4247/$ – see front matter © 2008 Elsevier B.V. All rights reserved.
possible to replace the expensive gyroscopes with gyro-free IMUs in an orientation estimation system while still achieving a requested estimation accuracy.
In the conventional gyro-free IMU designs, accelerometers have to be attached to an object at specific locations, in a 3D manner, to ensure IMU’s proper operation. As a consequence, it is not only expensive for the assembly but also susceptible to the alignment error that would deteriorate its sensing accuracy. In previous research[7], the author proposed a novel “coplanar” gyro-free IMU design, in which all the employed accelerometers, in-plane sensing accelerometers and out-of-plane sensing accelerometers, are situated on the same facet of the measurement unit. Current MEMS technology developments[8,9]show that it is possible to simultaneously fabricate the in-plane and out-of-plane sensing accelerometers on a substrate. Therefore, all the accelerometers in the coplanar gyro-free IMU can be fabricated in situ by these techniques. The fabrication cost and accelerometer alignment error can be minimized.
In this paper, we propose a novel orientation estimation system using single-axis linear accelerometers and 3-axis magnetic sensors. These linear accelerometers are deployed in a way to form a coplanar gyro-free IMU. As such, they can perform the integrated measurements of gravity force and angular velocity for the subsequent sensor fusion algorithms. The design procedures and stability analysis of this orientation estimation system are discussed in details. Furthermore, we discuss the relations between the angular velocity measurements and the observability[12]of the angle estimation. This leads to another issue of using fewer sensors in an orientation estimation system, for examples: removing x-direction gravity force sensor, z-direction magnetic sensors, etc.
2. Preliminary for orientation determination
2.1. Description for orientation angles
By defining the coordinates that is fixed at a point (inertial frame) and a coordinate that is rotating with a body in motion (body frame), one can use three rotation angles: roll (), pitch (), and yaw ( ) angles to describe the orientation of an object. This method is known as the “euler angle method”. With this method, one can find a transformation matrix that describes the relation of a vector represented in body frame and in inertial frame.
Cb i = R(x, )R(y, )R(z, ) =
1 0 0 0 cos sin 0 − sin coscos 0 − sin 0 1 0 sin 0 cos
cos sin 0 − sin cos 0 0 0 1 (1)
For the convenience of this application, the inertial frame is chosen to be the “navigation frame,” which is defined as follows: the origin is located at the center of the object with one axis (N-axis) points to the north pole of the earth, one axis (D-axis) points to the center of earth and the third axis (E-axis) points to the east. Therefore, this coordinate system is often referred to as the NED frame.
2.2. Angle measurement using earth gravity and magnetic field
According to the definition of NED frame, the vector form of gravity force and earth magnetic field, represented in NED frame, can be obtain as follows:
ig =
0 0 1T, im =
a 0 bT(2) where a and b are the strength of the magnetic field and their values vary with the locations of interest. Using the transformation matrix shown in Eq.(1), one can obtain the values of gravity force and magnetic field measured in the body frame.
bg = Cb i ig =
− sin sin cos cos cos
T bm = Cbiim ⇒ R(z, )im = R(y, )
TR(x, )Tbm ⇒
a cos −a sin bT=v1 v2 v3 T(3)
wherebg = [gxgygz]T
andbm = [mxmymz]T. The conventional approach of determining orientation angles is
=− sin−1(gx) tan−1 gy gz − tan−1v2 v1(4) According to above equations, the roll and pitch angles can be determined from the measurements of gravity force, and the yaw angle can be determined from the measurements of gravity force and earth magnetic field. In this approach, the parameters of earth magnetic field (a, b) is not required in determining orientation angles. And, when the pitch angle is 90◦, the measurements of gyand gzare both zero and thus this particular pitch angle cannot be determined from Eq.(4). This leads to the singularity problem of this approach.
2.3. Quaternion for the orientation presentation
Although the euler angle method is straightforward, it has the singularity problem during coordinates transformation[13]. Therefore, instead of using euler angles, many researchers use “Quaternion” to avoid this problem. Since these two methods are both developed to
Fig. 1. Schematic of inertial frame vs. body frame.
describe the orientation of an object, they are equivalent and the relations are described as follows[14,15]: = tan−1
2(q1q4+ q2q3) q2 4− q21− q22+ q23= sin−1(2(q2q4− q1q3)) = tan−1 2(q1q2+ q3q4) q2 1− q22− q23+ q24
(5)
Since three parameters are sufficient to describe the orientation of an object, these four quaternions must satisfy the “unity constraint” at all time.
qunity: q21+ q22+ q23+ q24= 1 (6)
Similar to the euler method, there is a transformation matrix with the quaternion method. Cib=
q2 1− q22− q23+ q24 2(q1q2+ q3q4) 2(q1q3− q2q4) 2(q1q2− q3q4) −q2 1+ q22− q23+ q24 2(q1q4+ q2q3) 2(q1q3+ q2q4) 2(−q1q4+ q2q3) −q21− q22+ q23+ q24 (7)3. Gyroscope-free IMU Theory
Fig. 1shows the relation between an inertial frame{i} and a body frame {b}. Rois a position vector from the center of the inertial frame to the center of body frame. Rjis a position vector from the center of the inertial frame to location j, andrjis a position vector from the center of body frame to location j.w is the rotation rate of the body frame. According to the coordinate transformation, the acceleration at location j can be written as follows:
¨Rj= ¨Ro+ ˙w × rj+ w × ( w × rj)+ 2 w × ˙rj+ ¨rj (8)
In the case that the body frame is rotated with the object in motion,brj, therjvector represented in body frame, is constant. Eq.(8)can be simplified to
i¨R
j=i¨Ro+ Cbibw ×˙ brj+ Cbibw × (bw ×rrj) i¨R
j=iFj+ig (9)
whereiFjis the specific force[16]applied at location j andig is the gravitational force, both of which are represented in the inertial frame. Assuming that a single-axis accelerometer is rigidly mounted on the object at the location j with the sensing direction ofj, the accelerometer output Ajcan be written as:
Aj= ¨Rj· j=b¨R j·bj =
(brj×b j)T bjbw˙ bFo+bg
+bT j · (bw × (bw ×brj)) (10)
When there are m accelerometers distributed on the object and no linear motion of the object (bFo= 0), these accelerometer measurements can be arranged in a vector form
⎡
⎣
A1 . . . Am⎤
⎦
= Jbw˙ bg
+
⎡
⎢
⎣
bT 1· (bw × (bw ×br1)) . . . bT m· (bw × (bw ×brm))⎤
⎥
⎦
J =⎡
⎢
⎣
(br1×b1) T b 1 . . . (brm×b m) T b m⎤
⎥
⎦
(11)Fig. 2. Cube-type gyro-free IMU vs. coplanar gyro-free IMU. The arrow denotes the sensing direction of each accelerometer. (→) denotes in-plane sensing accelerometers; (⊗) denotes out-of-plane sensing accelerometers.
If the matrix JTJ is invertible, the relation between the dynamics of an object in motion and the measurements from accelerometers located at various locations can be shown as follows:
b ˙ w bg
6×1 = (JTJ)−1JT
⎛
⎜
⎝
⎡
⎣
A1 . . . Am⎤
⎦
−⎡
⎢
⎣
bT 1· (bw × (bw ×br1)) . . . bT m· (bw × (bw ×brm))⎤
⎥
⎦
⎞
⎟
⎠
(12)3.1. Coplanar gyro-free IMU
The coplanar gyro-free IMU denotes that all the employed accelerometers are situated on the same facet of the measurement unit at the same elevation. The way to construct such a gyro-free IMU, using the minimum number of accelerometers, is to employ three accelerometers measuring the acceleration along the direction parallel to the plane surface of the measurement unit (in-plane sensing), and the other three accelerometers measuring the acceleration along the direction perpendicular to the plane surface of the measurement unit (out-of-plane sensing)[7]. The difference between a conventional cube-like gyro-free IMU and coplanar gyro-free IMU is depicted in Fig. 2.
3.2. Direct measurement of earth gravity using coplanar accelerometers
According to Eq.(11), the output of a linear accelerometer attached to a moving object depends on the linear acceleration, angular acceleration, and angular velocity of the object. The “direct measurement” of earth gravity using coplanar accelerometers denotes that using those accelerometer to retrieve the earth gravity (linear accelerations) without the information of angular motions.
For an in-plane sensing accelerometer located at [r cos ˛j r sin ˛j 0]Twith sensing direction [cos ˇjsin ˇj0]T, the accelerometer output, derived from Eq.(10), can be simplified to
⎡
⎢
⎢
⎣
. . . Ai−p j . . .⎤
⎥
⎥
⎦
=⎡
⎢
⎢
⎣
. . . ... cos ˇj sin ˇj . . . ...⎤
⎥
⎥
⎦
gx gy
+ r Hi−p
⎡
⎢
⎣
˙ wz w2 x+ wz2 w2 y+ wz2 wxwy⎤
⎥
⎦
Hi−p=⎡
⎢
⎢
⎣
sin(ˇj− ˛j) . . . − sin ˛jsin ˇj . . . − cos ˛jcos ˇj sin(˛j+ ˇj)⎤
⎥
⎥
⎦
T (13)wherebw = [wxwywz]T. Similarly, for an out-of-plane sensing accelerometer located at
r cos ˛jr sin ˛j0Twith sensing direction [0 0 1]T, the accelerometer output can be simplified to⎡
⎢
⎢
⎣
. . . Ao−p j . . .⎤
⎥
⎥
⎦
=⎡
⎢
⎢
⎣
. . . 1 . . .⎤
⎥
⎥
⎦
gz+ rHo−p⎡
⎢
⎣
˙ wx ˙ wy wxwz wywz⎤
⎥
⎦
Ho−p=⎡
⎢
⎢
⎣
. . .sin ˛j cos ˛j cos ˛j sin ˛j . . .
⎤
⎥
⎥
⎦
(14)Eqs.(13) and (14)indicate that the gxand gyonly relate to the in-plane sensing accelerometers, while gzrelates to the out-of-plane sensing accelerometers. Therefore, the design of the direct measurement of earth gravity can be separated into that using in-plane and out-of-plane sensing accelerometers, respectively.
To obtain values of gxand gy, the design task is to determine the locations and sensing directions of these in-plane sensing accelerometers such that Hi−pin Eq.(13)satisfies the following conditions.
U Hi−p= 0, rank
⎛
⎜
⎜
⎝
U⎡
⎢
⎢
⎣
. . . ... cos(ˇj) sin(ˇj) . . . ...⎤
⎥
⎥
⎦
⎞
⎟
⎟
⎠
= 2 (15)where U is the null space of Hi−p∗ and H∗i−pis the transpose conjugate of Hi−p. To achieve this task, the minimum number of accelerometers required is four, and these four accelerometers are distributed in the “pairs” configuration (Appendix A). The relation between gx, gyand measurements of these four accelerometers is described as follows:
gx gy
=
⎛
⎜
⎜
⎝
U⎡
⎢
⎢
⎣
. . . ... cos(ˇj) sin(ˇj) . . . ...⎤
⎥
⎥
⎦
⎞
⎟
⎟
⎠
−1 U⎡
⎣
Ai−p 1 . . . Ai−p 4⎤
⎦
(16)Analogously, one can prove that the minimum number of out-of-plane sensing accelerometers required for gzis three, and there is no constraint on the locations of these three accelerometers (Appendix A).
gz=
V 1 1 1 −1 V⎡
⎣
Ao−p 1 . . . Ao−p 3⎤
⎦
(17)where V is the null space of H∗o−pand Ho−p∗ is the transpose conjugate of Ho−p.
4. A novel orientation estimation method using coplanar gyro-free IMU
Since accelerometers are required in an orientation estimation system, we propose using gyro-free IMU to measure the gravity force and angular velocity so as to save expensive gyroscopes and increase the accuracy of the angle estimation. Here, we discuss two cases: one assumes that the parameters of earth magnetic field (a, b) is unknown; the other one assumes (a, b) is known.
4.1. (a, b) is unknown
When (a, b) is unknown, these two parameters are modelled as unknown constants. Therefore, the governing equations of the orientation estimation system can be written as follows:
b ˙ w 3×1= (J TJ)−1JT(1 : 3, :)⎛
⎜
⎝
⎡
⎣
A1+ n1 . . . A6+ n6⎤
⎦
−⎡
⎢
⎣
bT 1· (bw × (bw ×br1)) . . . bT 6· (bw × (bw ×br6))⎤
⎥
⎦
⎞
⎟
⎠
[ ˙q]4×1= 1 2q⎡
⎢
⎣
q4 −q3 q2 q3 q4 −q1 −q2 q1 q4 −q1 −q2 −q3⎤
⎥
⎦
wx wy wz ,˙ a ˙b
=
0 0
(18)
whereq is the 2-norm of the vector; J(1 : 3, :) is the first 3 row vectors of the J matrix; niis the measurement noise pertinent to the accelerometer Ai. And the output equations of the orientation estimation system are
bg = Cb iig = Cib
0 0 1T+ ng bm = Cb iim = Cib a 0 bT+ nm (19)where ngand nmare the noise associated with the accelerometers and magnetic sensors. Comparing to the conventional approach Eq.(4), this method does not have the singularity problem at the pitch angle of 90◦. Furthermore, it is rather straightforward to obtain the noise properties of the output equations. Thus, the employed sensor fusion algorithms can be simple.
The feasibility of this state estimation approach depends on whether the associated observability matrix is full rank. The observability matrix of a dynamic system is
Wo= ∇[z ˙z . . .] (20)
where z is the outputs of a system. In this case, the estimation system Eq.(18)consists of nine states: three from angular velocities, four from quaternions, and two from the parameters of earth magnetic field. However, if four quaternions can be estimated correctly, three angular velocities can be calculated from Eq.(18); this leads to six states to be estimated. Furthermore, since the orientation angles here are described by four quaternions instead of three euler angles, for the observability check, the system outputs z should include the unity constraint of quaternions in addition to the output equations shown in Eq.(19).
When the orientation angle to be measured is constant, the ˙z equals to zeros and the number of row vectors in an observability matrix is reduced. In turns, the rank of the observability matrix could be reduced and the angle estimation could be erroneous. Because the proposed design needs to estimate constant angles correctly, we check the rank of the observability matrix for Wo= ∇[z].
∇[z] =
∂z∂q ∂(a, b)∂z=
⎡
⎢
⎢
⎢
⎢
⎣
∂bm ∂q ∂bm ∂(a, b) ∂bg ∂q 0 ∂qunity ∂q 0⎤
⎥
⎥
⎥
⎥
⎦
=⎡
⎢
⎢
⎢
⎢
⎣
a∂C b i(:, 1) ∂q Cbi(:, 1) Cib(:, 3) ∂Cb i(:, 3) ∂q 0 0 ∂qunity ∂q 0 0⎤
⎥
⎥
⎥
⎥
⎦
(21)Since the transformation matrix Cibis always invertible, the above matrix can provide six independent column vectors as long as a = 0; four of them come from ∂z/∂q and the rest come from ∂z/∂(a, b). Therefore, it is full rank for the estimation of six states.
When removing any measurements out of the output equations, the rank of the resulting observability matrix is smaller than six. There-fore, in this case, we conclude that the measurements of 3-axis gravity force and 3-axis earth magnetic filed are necessary in determining three orientation angles of an object.
4.2. (a, b) is known
When (a, b) is known, the observability matrix consists of ∂z/∂q only, which is a 7× 4 matrix. Since, in this case, only four quaternions need to be estimated, there exists a chance to reduce the number of output equations, meaning that reduce sensor usage. For example, if the measurements of x-direction gravity force are removed from the output equations, the rank of the associated observability matrix would still be four mostly, except at two sets of singularity points. These singularity points can be obtained by
det
∂gy ∂q ∂gz ∂q ∂bm ∂q
∂gy ∂q ∂gz ∂q ∂bm ∂q
T = 0 ⇒ q1= q2, q3= q4 or q1= −q2, q3= −q4 ⇒ ( ) = ± tan−1 2q1q3 q2 3− q21 0◦ 90◦
(22)
Meanwhile, if another measurements of z-direction magnetic field are removed from the output equations, this would add another two sets singularity points, which are
q1= q4, q2= −q3; or q1= −q4, q2= q3
⇒ ( ) = ( 90◦ ± sin(4q1q2) 0◦) (23)
Looking at the euler angles of the above four sets of singularity points, they are four individual lines in a 3D space. If one more sen-sor measurements are removed from the output equations, for example: measurements of the y-direction magnetic field, the resulting observability matrix would lose rank at the following singularity points, which represent two surfaces in a 3D space.
(q1q3− q2q4)(q1q2+ q3q4)= 0 (24)
5. Angular velocity measurements and system observability
The angular velocity measured in body frame appears in the derivative of output equations, that is the ˙z in an observability matrix Eq. (20). Therefore, if these measurements can improve the observability of angle estimation, the (∂/∂q) ˙z should provide independent row vectors in addition to (∂/∂q)z. To investigate on this, the (∂, ∂q) ˙z are evaluated at the singularity points of the matrix (∂/∂q)z, and the rank of the observability matrix [(∂/∂q)z, (∂/∂q) ˙z]Tis examined accordingly.
Using the measurement of gravity force as an example, the derivative of gravity force measured in body frame can be obtained by the following: bg = −C˙ b i
d dtC i b bg, ˝ ≡ Cb i d dtC i b (25) Since Cibis a rotation transformation matrix, ˝ equals to−˝T. ˝ is then a skew symmetric matrix and can be represented as˝ =
0 −3 2 3 0 −1 −2 1 0 (26) When removing the measurements of x-direction gravity force out of the output equations, the gradient of the derivative of gravity force is∂ ∂q
˙ gy ˙ gz
=
⎡
⎢
⎣
2∂gx ∂q −3∂gx∂q⎤
⎥
⎦
+ gx⎡
⎢
⎣
∂2 ∂q ∂3 ∂q⎤
⎥
⎦
+⎡
⎢
⎣
gy∂1 ∂q −gz∂1 ∂q⎤
⎥
⎦
+ 1⎡
⎢
⎣
∂gy ∂q ∂gz ∂q⎤
⎥
⎦
(27)According to the equation above, as long as the entire angles to be measured do not stay in the singularity points obtained in Eq.(22), the matrix [ (∂ ˙gy/∂q), (∂ ˙gz/∂q) ]Tcan provide another two independent row vectors in addition to [ (∂gy/∂q), (∂gz/∂q) ]T. In that case,
Table 1
Coplanar configuration of seven single-axis accelerometers
Accel. number Locationbrj(x, y, z) Sensing directionb
j(x, y, z)
#1 r[cos(4/9) sin(4/9) 0] [cos(17/18) sin(17/18) 0]
#2 r[cos(10/9) sin(10/9) 0] [cos(−7/18) sin(−7/18) 0]
# 3x r[cos(−5/9) sin(−5/9) 0] [cos(−) sin(−) 0]
# 3y r[cos(/9) sin(/9) 0] [cos(/2) sin(/2) 0]
#4 r[cos(2/9) sin(2/9) 0] [0 0 1]
#5 r[cos(8/9) sin(8/9) 0] [0 0 1]
#6 r[cos(14/9) sin(14/9) 0] [0 0 1]
the observability matrix is full rank and one can successfully estimate four quaternions without the measurements of x-direction gravity force.
On the other hand, when the entire trajectory stays in the singularity points of Eq.(22), the rank of the observability matrix should be evaluated at such singularity points Qo:q1= q2, q3= q4, ˙q1= ˙q2, ˙q3= ˙q4. After some mathematics derivations, the following equations can be obtained. gx|Qo= 2|Qo= 3|Qo= 0 ∂ ∂q
˙ gy ˙ gz
Qo =
⎡
⎢
⎣
gy∂1 ∂q −gz∂1 ∂q⎤
⎥
⎦
Qo + 1⎡
⎢
⎣
∂gy ∂q ∂gz ∂q⎤
⎥
⎦
Qo ∂1 ∂q|Qo=⎡
⎢
⎢
⎣
8q1q3q1˙ + (−12q2 1− 4q23) ˙q3 8q1q3q1˙ + (−12q2 1− 4q23) ˙q3 −8q1q3q3˙ + (4q2 1+ 12q23) ˙q1 −8q1q3q3˙ + (4q2 1+ 12q23) ˙q1⎤
⎥
⎥
⎦
T⎡
⎢
⎣
∂gy ∂q ∂gz ∂q⎤
⎥
⎦
Qo =q3 q3 q1 q1 −q1 −q1 q3 q3
(28)
Therefore, ∂ ˙gy/∂q and ∂ ˙gz/∂q are just linear combinations of ∂bgy/∂q and ∂bgz/∂q. In turns, the observability matrix stays singular and thus the orientation estimation system cannot estimate four quaternions correctly.
6. Simulation results
This section presents simulation results for an example design: the seven single-axis accelerometers are distributed on a size of 0.125 m radius disk to form a coplanar gyro-free IMU. The location and sensing direction of each accelerometer are listed inTable 1. Assuming this gyro-free IMU and 3-axis magnetic sensors are mounted on an object and rotated with the object, the accelerometers and magnetic sensors would produce signals corresponding to the rotational motions of the object. These sensor outputs are then processed by the extended Kalman filter (EKF)[11]to determine the orientation angles of the object.
In the IMU design, the accelerometers that are marked A1, A2, A 3x, A 3y are the in-plane sensing accelerometers, whereas the ones marked A4, A5, A6 are the out-of-plane sensing accelerometers. The six accelerometers: A1, A2, A 3x, A4, A5, A6 are chosen to construct the coplanar gyro-free IMU. Furthermore, the A1 and A 3x pairs are used for the direct measurement of x-direction grav-ity force; the A2 and A 3y pairs are used for the y-direction gravity force; the A4, A5, and A6 are used for the z-direction gravity force. The data processing flow is shown inFig. 3. The trajectories to be followed are three angular velocities represented in body frame: 1.1 sin(22t), 2.2 sin(22t), 2.2 sin(22t) rad/s. The earth magnetic field represented in the NED frame is [0.4243 0 0.4243]T G. Without loss of generality, the noise associated with each measurement is assumed to be white, with the standard devia-tion of 0.1 m/s2for each accelerometer and 0.01 G for each magnetic sensor. This leads to the signal-to-noise ratio of 50 for the
Fig. 4. The outputs of seven accelerometers and three magnetic sensors along a trajectory.
sensor measurements. The measurements of seven accelerometers and three magnetic sensors along the trajectory are shown in Fig. 4.
Fig. 5shows the estimation of euler angles and the parameters of earth magnetic field. As evident in the plots, the estimated values quickly converge to their respective correct values. Thanks to the extended Kalman filter, after 4 s, the standard deviations of the estimated values decrease to 0.18◦, 0.1◦, and 0.5◦for three euler angles and 10−5G for the parameters of magnetic field. Furthermore, if the standard deviations are calculated for the last two seconds, they are 0.11◦, 0.09◦and 0.2◦for three euler angles.
Fig. 6shows the state estimation for the case where the orientation to be measured are three euler angles of 8◦, 90◦, and 20◦, which are equivalent to four quaternions of−0.074, 0.703, 0.074, and 0.703. Unlike conventional methods, the proposed method can still estimate orientation angles correctly for the pitch angle of 90◦. In this particular case, the state estimation is shown for quaternions instead of euler angles, because our computation codes were written for quaternions and the transformation between euler angles and quaternions is singular at the pitch angle of 90◦.
Fig. 7shows the angle estimation for the case where the earth magnetic field in NED frame is known. Furthermore, the sensor measure-ments of x-direction gravity force and z-direction earth magnetic field are removed from the output equations. To increase the visibility of plots, the singular values of the observability matrix, [∂z/∂q] in Eq.(21), are drawn in log scale. There are four singular values for four quaternion estimation; two of them are roughly identical at the value of 20 and thus only 3 lines are visualized. As shown in the plot, the
Fig. 6. Estimation of quaternions and earth magnetic field for the case where the corresponding pitch angle is at 90◦. Estimated values converge to their respective correct values (target).
singular values of the observability matrix are mostly nonzero, except at some distinct points. And, the estimation of euler angles are still accurate.
Fig. 8shows the angle estimation for the case where the measurements of x-direction gravity force and y, z-direction earth magnetic field are all removed from the output equations. According to the simulation results, the estimation of euler angles are erroneous. The associated observability matrix [∂z/∂q] indicates that one of its singular values remains zero along the entire trajectory.
Fig. 9shows the angle estimation for the case where the measurements of x-direction gravity force are removed from the output equations, and the angle to be measured is [(t), 0, 90]. This trajectory corresponds to the singularity points wherein the measurements of x-direction gravity force are removed. Different fromFig. 7, the associated observability matrix has one singular value of zero along the entire trajectory and the angle estimation is erroneous.
Table 2lists the accuracy of angle estimation for the cases where different accuracies of angular velocity measurements are included in an orientation estimation system. According to simulation results, the accuracy of the angle estimation is greatly improved by angular
Fig. 7. Estimation of orientation angles for the case where the sensor measurements of x-direction gravity force and z-direction earth magnetic field are removed from the
Fig. 8. Estimation of orientation angles for the case where the sensor measurements of x-direction gravity force and y, z-direction earth magnetic field are removed from the
output equations. Estimated values deviate from their respective correct values (target).
Fig. 9. Estimation of orientation angles for the case where the entire trajectory falls in the singularity points. Estimated values do not converge to their respective correct
values (target).
Table 2
The accuracy of angle estimation varies with the accuracy of incorporated angular velocity sensors
Sensor noise Roll (◦) (EKF/(r-o-t)) Pitch (◦) (EKF/(r-o-t)) Yaw (◦) (EKF/(r-o-t))
Direct measurement 0.594 0.569 1.507 Gyro-free 0.1 (m/s2) 0.11 0.09 0.2 Gyro std 0.03 (rad/s) 0.025/(0.05) 0.029/(0.049) 0.027/(0.08) Gyro std 0.1 (rad/s) 0.066/(0.09) 0.078/(0.089) 0.084/(0.14) Gyro std 0.2 (rad/s) 0.113/(0.12) 0.128/(0.126) 0.152/(0.206) Gyro std 1 (rad/s) 0.280/(0.27) 0.279/(0.268) 0.478/(0.45) Gyro std 10 (rad/s) 0.563/(0.55) 0.485/(0.53) 1.202/(1.164)
velocity measurements. Furthermore, the more accurate of angular velocity measurements; the more accurate of the angle estimation. The proposed gyro-free method improves the accuracy over the one that does not include any angular velocity measurements, as the “direct measurement” shown in Table, by 540%, 632%, and 754% for the roll, pitch, and yaw angle.
7. Discussion
Figs. 9 and 7indicate that the angular velocity measured in body frame improves the observability of the angle estimation when the angles to be measured do not remain in the singularity points, which are the singularity points of the Jacobian matrix of the out-put equations Eq.(19). These simulation results agree well with the analysis work stated in Section5. Together with the simulation results of estimation accuracy shown inTable 2, it can be concluded that the information of the angular velocity, measured in body frame, can greatly improve the accuracy of the angle estimation and improve the observability of angle estimation only under certain circumstances.
Due to the nonlinearity of this particular sensor fusion system, one must go through complicated numerical simulations to obtain the relation between the accuracy of the incorporated angular velocity measurements and that of the estimated angle. However, it would be very useful to engineers if there exists a rule-of-thumb (r-o-t) that can predict this relation. Here, we use a linear, first-order system together with Kalman filter to obtain the rule-of-thumb. The derivation is shown in (Appendix B). As listed in Table 2, the values predicted by rule-of-thumb may not be accurate but the orders of the predicted values are correct. Particularly, when the incorporated gyroscope is noisily, the value predicted by rule-of-thumb can be fairly close to the results obtained from numerical simulations. This is because when the incorporated gyroscope is noisy, its measurements play a less important role in determining the orientation angle. Therefore, the deviation between the simplified linear system and the original nonlinear system is reduced.
8. Conclusions
This paper presents the design procedures and stability analysis of a novel orientation estimation system. This design does not have the singularity problem at the pitch angle of 90◦and can work with a simple sensor fusion algorithm. A design example shows that it can achieve the sensing accuracy of 0.11, 0.09, and 0.2◦for roll, pitch, and yaw angle; this leads to the 540%, 632%, and 754% accuracy improvement over the one that does not employ any sensor fusion design.
The observability matrix is an indicator for a feasible orientation estimation system design. Therefore, as long as the associated observability matrix maintains its rank, it is possible to use fewer sensors in an orientation estimation system.
The analysis work shows that including the angular velocity measurements in an orientation estimation system can greatly improve the accuracy of the angle estimation and improve the observability of that only when the angles to be measured do not remain in the singularity points, which are the singularity points of the Jacobian matrix with respect to the accelerometer and magnetic sensor measurements. Furthermore, the accuracy of the angle estimation can be predicted by the proposed rule-of-thumb to some extent.
Appendix A. Direct measurement of earth gravity using single-axis, coplanar accelerometers
A.1. Design of in-plane sensing accelerometers
The task can be achieved by finding the locations and sensing directions of several in-plane sensing accelerometers such that U is the null space of Hi−pand consists of two independent row vectors. For any two accelerometers located at r[cos ˛isin ˛i0]Tand r
cos ˛jsin ˛j0T with sensing directionscos ˇisin ˇi0 Tand
cos ˇjsin ˇj0 T, the rank of Hi−pof one can be achieved by
˛i= ˛j+ ˇi= ˇj or ˇi= ˇj+ (A.1)
Since it must be single-axis accelerometers in this design, meaning that there is only one accelerometer at a location, it is impossible to achieve the rank of Hi−pof one with three accelerometers. Therefore, the minimum number of accelerometers required could be four; this leads to the rank of Hi−pof two. In turns, one can arbitrary design two pairs of in-plane sensing accelerometers, as long as the accelerometers in each pair are located 180◦apart.
A.2. Design of out-of-plane sensing accelerometers
Follow the same design procedures, with two out-of-plane sensing accelerometers, one can prove that these two accelerometers must be 180◦apart to achieve the rank of Ho−pof one. However, this leads to V· [1 1]T
= 0, which violate a similar rule described in Eq.(15). Therefore, one needs to use three out-of-plane sensing accelerometers, with which the rank of Hout-of-planeis two and numbers of row vectors of V is one. Since, with the number of out-of-plane sensing accelerometers larger than two, the rank of Ho−pis always two, these three accelerometers can be placed at any locations.
Appendix B. Accuracy of the state estimation
If the euler angle transformation is defined as the so called “3-2-1,” the relation between angular velocities represented in body frame and euler angle change rates can be obtained as follows:
wx wy wz = ˙ 0 0 + R(x, ) 0 ˙ 0 + R(x, )R(y, ) 0 0 ˙⎡
⎣
˙˙ ˙⎤
⎦
=1 sin tan cos tan
0 cos − sin
0 sin sec cos sec
wx
wy wz
(B.1)As shown in the equation above, it is extremely difficult to predict the accuracy of angle estimation not only because the system is nonlinear but also the noise variance depends on the angles to be measured.
Here, we discuss the accuracy of state estimations for a first-order, one-state linear system, which is described by a pure integration. x(k + 1) = x(k) + nq
y(k) = x(k) + nr (B.2)
Follow the standard procedures of the Kalman filter construction, one can obtain the following equations for the above system. P(k + 1|k + 1)R(k + 1)
R(k + 1) − P(k + 1|k + 1)= P(k|k) + Q(k) (B.3)
where R(k) and Q (k) are the variance of the noise nrand nqat time k; P(k|k) is the variance of the estimated state at time k. If this states estimation is stable, P(k+ 1|k + 1) should converge to P(k|k); this leads to the following:
P2(k|k) + P(k|k)Q(k) − Q(k)R(k + 1) = 0 (B.4)
Therefore, if the Q (k) and R(k+ 1) are known, one can solve the above second order polynomial function and predict the estimation accuracy for this simple system.
References
[1] M.J. Caruso, Applications of Magnetoresistive Sensors in Navigation Systems, Honeywell Inc. Technical Reports.
[2] W. Ang, P. Khosla, C. Riviere, Kalman filetering for real-time orientation tracking of handheld microsurgical instrument, Int. Conf. Intell. Robots Syst. (2004) 2574–2580. [3] A. Gallagher, Y. Matsuoka, W.T. Ang, An efficient real-time human posture tracking algorithm using low-cost inertial and magnetic sensors, in: Proceedings of 2004
IEEE/RSJ, 2004, pp. 2967–2972.
[4] J.L. Marins, X. Yun, E.R. Bachmann, R.B. McGhee, M.J. Zyda, An extended kalman filter for quaterion-based orientation estimatoin using MARG sensors, in: Proceedings of 2001 IEEE/RSJ, 2001, pp. 2003–2011.
[5] Q. Wang, M. Ding, P. Zhao, A new scheme of non-gyro inertial measurement unit for estimating angular velocity, IECON 2 (2003) 1564–1567. [6] A. Kim, M.F. Golnaraghi, A quaternion-based orientation estimation algorithm using an inertial measurement unit, PLANS (2004) 268–272. [7] T.L. Chen, S. Park, MEMS SoC: observer-based coplanar gyro-free inertial measurement unit, J. Micromech. Microeng. (2005) 1664–1673. [8] H. Qu, D. Fang, H. Xie, A single-crystal silicon 3-axis CMOS-MEMS accelerometer, Sens. Proc. IEEE (2004) 661–664.
[9] H. Kim, S. Seok, H. Kim, A. Choi, K. Chun, Inertial-grade out-of-plane and in-plane differential resonant silicon acceleromters (DRXLs), Transducers’05, 172–175. [11] Y. Bar-Shalom, X.R. Li, T. Kirubarajan, Estimation with Applications to Tracking and Navigation, John Wiley & Sons, 2001, p. 371.
[12] H.J. Marquez, Nonlinear Control Systems, John Wiley & Sons, 2003, p. 296.
[13] G. Vladimir, Spececraft Attitude Dynamic and Control, Krieger Publishing Company, 1991.
[14] S. Ronnback, Development of a INS/GPS Navigation Loop for an UAV, Lulea University of Technology, Department of Computer Science and Electrical Engineering, M.S. Thesis, 2000, pp. 1–80.
[15] K.N. Vikas, Integration of Inertial Navigation System and Global Positioning System using Kalman Filtering, Indian Institute of Technology, Department of Aerospace Engineering, 2004, pp. 20–33.
[16] C.W. Tan, S. Park, K. Mostov, P. Varaiya, Design of gyroscope-free navigation systems, in: IEEE Fourth International Conference on Intelligent Transportation Systems, 2001, pp. 286–291.
Biographies
Chung-Fu Kao received his MS degree in mechanical engineering form National Chiao Tung University, Shinchu, Taiwan in 2006. Currently, he is employed in Philips &
Lite-On Digital Solution Corporation.
Tsung-Lin Chen received his BS and MS degrees in power mechanical engineering from National Tsing Hua University, Shinchu, Taiwan in 1990 and 1992, respectively. He
received his PhD degree in mechanical engineering from the University of California, Berkeley in 2001. From 2001 to 2002, he joined Analog Devices Inc. as a MEMS design engineer. Since 2003, he has been with the Department of Mechanical Engineering, National Chiao Tung University, Shinchu, Taiwan. Currently, he is an assistant professor. His research interests include MEMS and controls.
