Journal of Robotics, Networking and Artificial Life, Vol. 3, No. 2 (September 2016), 102-106

Attitude/Position Estimation of Rigid-Body using Inertial and Vision Sensors
Shihao Sun

The Seventh Research Division and the Center for Information and Control, School of Automation Science andElectrical Engineering, Beihang University (BUAA)
Beijing, 100191, China
Yingmin Jia

The Seventh Research Division and the Center for Information and Control, School of Automation Science andElectrical Engineering, Beihang University (BUAA)
Beijing, 100191, China
E-mail: jxcrssh@126.com, ymjia@buaa.edu.cn
www.buaa.edu.cn

Abstract
This paper is concerned with the attitude/position estimation of a rigid-body using inertial and vision sensors. By
employing the Newton-Euler method, a kinematic model is developed for the rigid-body by treating the inertial
measurements as inputs. Based on the coordinate transformation, a nonlinear visual observation model is proposedby using the image coordinates of feature points as observations. Then the Extended Kalman filter (EKF) is utilized
to estimate the attitude/position recursively. The effectiveness of the proposed algorithm is evaluated by simulation.
Keywords: attitude and position estimation, EKF, inertial sensor, vision sensor.

1. Introduction
Accurate attitude/position estimation of rigid-body hasreceived considerable attention in the past decades1-8.
This is partly due to the fact that it is generally requiredin many typical applications such as estimating themotion of a robot end-effector9, navigation for small
unmanned aerial vehicle10, spacecraft relative
navigation in rendezvous11 etc. The widely used way ofobtaining position and orientation is using an inertial
measurement unit (IMU) as the navigation sensor,which consists of a tri-axis gyroscope and a tri-axis
accelerometer. However, integration over a long time
period may lead to unbounded estimation errors if
noises, offsets, scale errors and uncertainty innavigation model are present. Using vision as astandalone sensor for attitude/position estimation is alsoquite a standard way12, because of the ability to sense
the actual attitude/position without accumulative errors.

However, vision sensors can only sense the actual
position but not the velocity and accelerate. Therefore,
an underlying dynamic model for the motion of rigidbody is needed for accurate estimation when we use
vision sensor alone.
Motivated by the discussions above, thecombination of the vision and inertial sensors has beenrecognized as a promising choice for accurate attitude
and position estimation. For example, rigid body pose
estimation using inertial sensors and a monocularcamera is considered in Ref. 13, and it is shown how
rotation estimation can be decoupled from position
estimation. In Ref. 14, Chen studied the problem of thepose estimation of robotic end-effector with inertial andSE(3) measure-ments. Among these literature, themeasurements of vision sensor are the actual attitude
and position that can be obtained by using machine
vision algorithms like PNP, stereo-vision. Although thePublished by Atlantis Press
Copyright: the authors
102

Journal of Robotics, Networking and Artificial Life, Vol. 3, No. 2 (September 2016), 102-106

Shihao Sun, Yingmin Jia

observation model is linear in such way, the expressionof observation noises is complicated according to thecomplicated machine vision algorithms and costs huge
computations. An alternative way is applying the image
coordinates of feature points as observations directly.
In this paper, we attempt to estimate the orientationof a rigid-body using inertial and vision sensors. The
inertial measurements are treated as inputs fordeveloping the kinematic model, and a novel non-linear
observation model is proposed by using the image
coordinates of feature points. The EKF is utilized toaddress the nonlinear filtering problem. Simulation
results are provided to evaluate the performance of theproposed algorithm.
2. Problem Statement

where E ωe ∈ R 3 is the angular velocity of the effector as
viewed in the effector frame and0 sin γ cos β=T ( Φ ) 0cos γ
1 sin γ tan βcos γ cos β − sin γ cos γ tan β (3)Discretization of equations (1) and (2) is needed todesign a Kalman Filter. Assume that B ae and E ωe areconstants in each sampling interval  kT , ( k + 1) T  withsampling period Ts .
The state transition can be approximated by:sBpe ( k +=1)

Bpe ( k ) + Ts B ve ( k ) +Bve ( k +=1)

Bve ( k ) + Ts B ae ( k )sTs2 Bae ( k )2Φ ( k + 1) = Φ ( k ) + TsT ( Φ ( k ) ) E ωe ( k )(4)

2.2. Transition models using IMU measurements
The IMU in Fig. 1 consists of a tri-axis gyroscope and atri-axis accelerometer that output:
ωimu = I ωi + I mωaimu =Iai + I g + I m a(5)where ωimu and aimu are IMU measurements, mω and ma
are measurement noises, and g is the gravity vector.
According to the rigid kinematic theorem, theangular velocity and acceleration relations between
point pi and pe are as follows:
Fig. 1. System configuration and coordinate framesEωe = E RI I ωi

In this paper, we consider the case in which the motionof a rigid-body (noted as effector) is controlled by theABB-120 robot as shown in Fig.1. The effector isequipped with a strap-down IMU and four infrared LED
feature points. The vision sensor is a camera equippedwith an infrared filter, which is fixed at a certain
location in the base frame.

Bae =2.1. Effector Kinematics
As shown in Fig. 1. Denote B pe ∈ R 3 as the effector
position in the base frame, evolving as:
B(1)=p e B=ve B ve B ae
where B ve ∈ R 3 and B ae ∈ R 3 are referred as the translational

velocity and acceleration of SCP in the base frame,respectively.
Denote Euler (3-2-1) rotation angles Φ = [α β γ ]as the effector attitude observed in the base frame,according to attitude kinematics:
Φ = T ( Φ ) E ωe
(2)

B

RE E RI ( I ai + S ( I αi ) I rei + S ( I ωi ) S ( I ωi ) I rei )(6)where E RI is a constant orientation matrix referred as theIMU frame attitude observed in the effector frame; I rei isthe position vector of pe relative to pi in IMU frame; I αiis the angular acceleration of the IMU, according to theassumption that E ωe is constant in each samplinginterval, then I αi ( k ) = 0 ; B RE is the orientation matrix
referred as the end-effector attitude observed in the baseframe, which is given bycos α cos βB=RE  sin α cos β − sin β− sin α cos γ + cos α sin β sin γ
cos α cos γ + sin α sin β sin γ
cos β sin γsin α sin γ + cos α sin β cos γ ;− cos α sin γ + sin α sin β cos γ cos β cos γand S ( • ) is the cross product operator transforms avector c = [c c c ] to a skew-symmetric matrix
T123

 0
=
S ( c )  c3
 −c2T−c 30c1c2 −c1  .
0 Given (5) and (6) at the sampling interval, we havePublished by Atlantis Press
Copyright: the authors
103

Journal of Robotics, Networking and Artificial Life, Vol. 3, No. 2 (September 2016), 102-106

Attitude/Position Estimation of Rigid-Body
E=ωe ( k )B

ERI ωimu ( k ) − E RI I mω ( k )Z i ( k )= hi  X ( k ) , E pi  + Vi (k)ae ( k ) =RE ( k ) E RI aimu ( k ) + B RE ( k ) E RI S ( ωimu ( k ) ) S ( ωimu ( k ) ) I rei − B gB( (− B RE ( k ) E RI S S ( ωimu ( k ) ) I rei + S ( ωimu ( k ) ) S ( I rei ) I mω ( k )+ B RE ( k ) E RI S ( I mω ( k ) ) S ( I mω ( k ) ) I rei − B ma

(7)
Then the state transition equation (4) can be
rewritten asX ( k + 1) =φ  X ( k ) , U ( k )  + Γ  X ( k ) , U ( k )  W ( k ) (8)where X ( k ) =ωimu ( k ) aimu ( k )  pe ( k ) ve ( k ) Φ ( k )  , U ( k ) =BTBW ( k ) =  I mω ( k ) I ma ( k ) S ( I mω ( k ) ) S ( I mω ( k ) ) I rei TT,Cov {W ( k ) ,W ( j )} = Qkδ kj

Ts2 B ˆB
BEIB pe ( k ) + Ts ve ( k ) + 2 RE ( k ) RI aimu ( k ) + S ( ωimu ( k ) ) S ( ωimu ( k ) ) rei − g B=φ  X ( k )  ve ( k ) + Ts B Rˆ E ( k ) E RI aimu ( k ) + S ( ωimu ( k ) ) S ( ωimu ( k ) ) I rei − B gΦ ( k ) + TsT ( Φ ( k ) ) E RI ωimu ( k )(((2 Ts 2 BEII
 − 2 Ts RE ( k ) RI S S ( ωimu ( k ) ) rei + S ( ωimu ( k ) ) S ( rei )BEIIΓ  X ( k ) =  −Ts RE ( k ) RI S S ( ωimu ( k ) ) rei + S ( ωimu ( k ) ) S ( rei )
TsT ( Φ ( k ) ) E RI)( ())( ())())) − T2 T2 R ( k ) R  .
2s2sBEE−Ts0I


Ts RE ( k ) RI 0BE

2.3. Observation models of Camera
The rigid body in Fig. 1 contains four feature points
TEnoted as E pi ==pix E piy E piz  , i 1 4 , and the relation

between the fixed camera and the base frame are notedas the orientation matrix C RB and the translation vector
Crbc . By using the homogeneous transform matrices
between the frames of Base, Effector and Camera, weobtain pi   1 Crbc   REpe   pi  RB
=   i 1 4 (9)1  01  1  0CCBBE

According to the camera pinhole model with theprojective geometry, we obtainp=ui f x C ix + u0piz
CCp=vi f y C iy + v0piz(10)

i = 1 4

where f x and f y are pixel magnification factors,( u0 , v0 ) denotes the image coordinate of the camera's
principal point, and ( ui , vi ) is the coordinate of thefeature point pi in the image plane. In view of (9) and(10), denote Z i ( k ) as the image coordinate ( u ( k ) , v ( k ) )at time k , then the observation model of camera can be
written asi

where hi is a 2-dim function derived by replacing pi inequation (10) using equation (9), V (k) is themeasurement
noiseandits
covariance
isCov {V ( k ) ,V ( j )} = R δ .
C))

(11)

i= 1 4iii

kikj

3. Estimation based on EKF
Since the state transition model (8) and the observation
model (11) are both nonlinear, the EKF is used toupdate the effector motion state estimate Xˆ ( k k ) and its
estimation error covariance matrix P ( k k ) .
The EKF is implemented as follows:1) Prediction. Denote Xˆ ( k + 1 k ) P ( k + 1 k ) Zˆ ( k + 1 k ) as
the one step predictions of estimation, covariance matrix
and observation at the time k + 1 .
Then as to (8), they can be obtained(12)Xˆ ( k + 1 k ) =φ  Xˆ ( k k ) , U ( k ) iP ( k + 1 k ) = Ψ  k + 1 k  P ( k k )Ψ T  k + 1 k + Γ  Xˆ ( k k ) , U ( k )  Qk Γ T  Xˆ ( k k ) , U ( k ) ∂φ  X ( k ) , U ( k ) where Ψ  k + 1 k  =
X ( k ) = Xˆ ( k k )∂X ( k )

(13)

(14)
Zˆ ( k + 1 k=) h  Xˆ ( k + 1 k ) , E pi 
TTwhere Zˆ =  Zˆ 1 Zˆ 2 Zˆ 3 Zˆ 4  and h = [ h1 h2 h3 h4 ] represent

four feature points.
2) Observation. As the feature points are infrared LED
and the camera is equipped with an infrared filter, wecan easy obtain the coordinates Z i ( k + 1) of the featurepoint pi in the image plane at the time k + 1 .
3) Update. When the new image is obtained at time k + 1, the filter can be computed as(15)K ( k + 1)= P ( k + 1 k ) H ( k + 1) O ( k + 1)
T1)  H ( k + 1) P ( k + 1 k ) H T ( k + 1) + diag [ Rki ]i=14 
where O ( k +=−1∂h  X ( k + 1) , E pi .
H ( k + 1) = X ( k + 1=
∂X ( k + 1)) Xˆ ( k + 1 k )Then the state estimate Xˆ ( k + 1 k + 1) and its
estimation error covariance matrix P ( k + 1 k + 1) at timek + 1 can be computed byXˆ ( k + 1 k + 1=) Xˆ ( k + 1 k ) + K ( k + 1)  Z ( k + 1) − Zˆ ( k + 1 k ) (16)P ( k + 1 k + 1) = I − K ( k + 1) H ( k + 1)  P ( k + 1 k ) (17)Published by Atlantis Press
Copyright: the authors
104

Journal of Robotics, Networking and Artificial Life, Vol. 3, No. 2 (September 2016), 102-106

Shihao Sun, Yingmin Jia

4. Simulation results
As stated in section 2, the effector is controlled by therobot, in which the revolute joints are programmed asq=π180image coordinates (Pixels) of feature points asobservations.

t  ( rad ) t ∈ [ 0,100π ] .
 100 [ 40 10 −5 60 8 10] cos And the robot forward kinematics with nominal D-H
parameters is used to provide the attitude /position of
the effector relative to base frame, which serve as thereal. The sampling period of IMU and camera are both
set as 0.1s , ignored the different sampling rate of thesensors. The noises of the IMU and camera sensors areassumed as mω ~ N ( 0,0.12 ) ma ~ N ( 0,2.52 ) V ~ N ( 0,5 ) .
The plots in Fig.2 and Fig.3 show the estimate results
based on EKF and transition models only use IMU,respectively. The simulation results suggest theeffectiveness of the proposed method.
2Fig. 4. The RMSE in position and attitude

Because of the complex computation, the measurement noises covariance matrices in measurement modelobtained by PNP algorithm are not calculated based onthe pixels noises of the camera, it is set based on ourexperience in this simulation. Besides, the errors of theattitude measure are shown in Fig.5. It’s clear from thefigure that the error is non-Gaussian.

Fig. 5. The errors of the attitude measure
Fig. 2.Estimate of Effector Motion Based on EKF

Fig. 3. Estimate of Effector Motion Use IMU Only

Notice from (7) that the IMU’s acceleration
measurement is affected by the effector acceleration andthe gravity, and the effector acceleration is muchsmaller than gravity in this simulation. Thus, it is very
sensitive to noise, and lead to the estimate errors ofposition use IMU only are very large as shown in Fig.3.
The measure model Z=( k ) X ( k ) + V ( k ) , which canbe obtained by using the PNP algorithm (refer tosolvepnp function in OpenCV), is also applied in EKF
to compare against the proposed observation models
noted as Pixels Measure. To illustrate the performanceof the observation models, the root mean square error(RMSE) in position and attitude are shown and thesimulation results are derived from 100 Monte Carloruns. The RMSE in position and attitude are shown inFig. 4. The simulation results suggest that theperformance of the EKF can be improved by using the5. Conclusion
This paper investigated the attitude/position estimationof a rigid-body by using a measurement system consistof an inertial sensor and a vision sensor, and an EKF isapplied to fuse these measurements. The motion states
are propagated in time using the inertial measurements
processed through the Newton-Euler equations, and thepixels coordinates of feature points are used asobservation directly, which is different from using
position and attitude coordinates in the camera frame.
Simulation results suggest that the performance of theEKF can be improved by using the pixels coordinates offeature points as observations than calculating position
and attitude coordinates through PNP algorithm. Future
work would focus on the on-line tracking estimation
experiments.
Acknowledgements
This work was supported by the National Basic
Research Program of China (973 Program:
2012CB821200, 2012CB821201) and the NSFC
(61134005, 61327807, 61521091, 61520106010).

Published by Atlantis Press
Copyright: the authors
105

Journal of Robotics, Networking and Artificial Life, Vol. 3, No. 2 (September 2016), 102-106

Attitude/Position Estimation of Rigid-Body

References
1. L Chen, and Y Jia, Variable-poled Tracking Control of a
Two-wheeled Mobile Robot Using Differential Flatness,
Journal of Robotics, Networking & Artificial Life, 1(1)(2014), 12-16.
2. J He, and Y Jia, Adaptive Sliding Mode Control for
Magnetic Levitation Vehicles, Journal of Robotics,
Networking & Artificial Life, 1(2) (2014)169-173.
3. X Lu, and Y Jia, Attitude Reorientation of Spacecraftwith Attitude Forbidden Zones, Journal of Robotics,
Networking & Artificial Life, 2(1) (2015)13-16.
4. C Yang, and Y Jia, Adaptive Multiple-Model Control of
A Class of Nonlinear Systems, Journal of Robotics,
Networking & Artificial Life, 2(2) (2015)69-72.
5. M Duan, and Y Jia, Adaptive Sliding Mode Control for A
2 DOF Magnetic Levitation System with Uncertain
Parameters, Journal of Robotics, Networking & Artificial
Life, 2(4) (2016)263-267
6. Y Jia, Robust Control with Decoupling Performance for
Steering and Traction of 4WS Vehicles under VelocityVarying Motion, IEEE Transactions on Control Systems
Technology, 8(3)(2000)554-569.
7. Y Jia, Alternative Proofs for Improved LMI Representations for the Analysis and the Design of ContinuousTime Systems with Polytopic Type Uncertainty: A
Predictive Approach, IEEE Transactions on AutomaticControl, 48( 8)(2003) 1413-1416.

8. Y Jia, General Solution to Diagonal Model Matching
Control of Multi – Output-Delay Systems and Its
Applications in Adaptive Scheme, Progress in NaturalScience, 19( 1)( 2009) 79-90.
9. Wang C, Chen W, Tomizuka M., Robot end-effector
sensing with position sensitive detector and inertial
sensors, in Proc. IEEE Int. Conf. Robotics andAutomation. (Minnesota, USA, 2012), 5252-5257.
10. Williamson, Walton R., et al, Sensor Fusion Applied toAutonomous Aerial Refueling, J. Guid Control Dynam.
32(1) (2009) 262-275.
11. Zhang G, M. Kontitsis, et al, Cooperative Relative
Naviga-tion for Space Rendezvous and Proximity
Operations using Controlled Active Vision, J. Field
Robot. 00(0) (2015) 1–24.
12. D. B. Gennery. Visual Tracking of Known Three Dimensional Objects, Int J Comput Vision, 7(3) (1992), 243270.
13. H Rehbinder, B.K. Ghosh, Pose estimation using line
based dynamic vision and inertial sensors. IEEE T
Automat Contr, 48(2) (2003) 186 - 199.
14. Chen X, et al. Pose estimation of robotic end-effectors
under low speed motion using EKF with inertial andSE(3) measurements, in Proc. IEEE Int. Conf. AdvancedIntellig-ent Mechatronics,(Busan, Korea 2015) 15851590.

Published by Atlantis Press
Copyright: the authors
106