ating a 
3.1.1. In 
ject is 
it) and 
In the 
ctor is 
known 
ist and 
rors in 
present 
Vision 
llowing 
to the 
ect to 
ject is 
to the 
2s the 
reached 
to the 
own as 
nd is 
rer the 
2 wrist 
Les; 
in-hand 
en two 
a graph 
can be 
W, 
S Te 
Ts is 
n fact, 
obtained 
  
  
  
  
  
  
  
  
Br WC Oi 1-8, = 1 B7-1 
Tw = Te: Ta: To To: Ts 
This procedure leads to an estimation of the 
kinematics of the manipulator using Vision and is in 
general known as camera calibration. This approach can 
be used to verify the real position and orientation of 
the wrist. 
second case: it is supposed that transform DT i is 
known (the kinematics model is used) and transform en. 
is computed. Once NT re Br are also known, or 
can be obtained using the following transform equation: 
S O C,-1 W,-1 B,-1 B 
Tof Te T Te: T Ts 
The object must be located by the Vision system in 
order to be grasped by the wrist; this is called 
object location problem. In this case the accuracy of 
the position and orientation of the object is bounded 
by the error in the kinematics model. 
4. FILTERING 
4.1 Introduction 
There are several parameters estimation methods, 
such as the well known Least Squares and the more 
recent Kalman Filtering. In Least Squares it is assumed 
that parameters are not time dependent and observation 
does not follow an a priori probability distribution. 
In this paper, due to the dynamic nature of the 
problem, Kalman Filtering will be used for parameters 
estimation . In the next section a short review of 
Kalman Filtering is presented. A more detailed 
description can be obtained, for example, in Jazwinski 
(1970) and Miller & Leskiw (1987). 
4.2 Kalman Filtering 
Equation (4.2.1) describes a discrete and dynamic 
stochastic system: 
X17 * (x, t t) +I (x,, t) Ww (4.2.1) 
k+1’ k+1 
where: x is the n-vector state at AE 
ó is a n-vectorial state transition 
function; 
r is a (nxr) matrix; 
Ww is a r-vector, white Gaussian sequence, 
7. N(0,Q ) , usually called state 
transition noise; 
Let z, be the observation vector: 
z = h(x ,t)+n km 2 (4.2.2) 
k k k k 
where: 
ze are the observations at to 
n is the vector of measurement noise, 
nc N(O,R ); 
Equation (4.2.2) describes the measurement model. 
When both the system model and the measurement 
model are linear, then: 
ó (X, t ).x (4.2.3) 
t) = F(t.» t E 
k+1’ 
and; 
= 4.2.4 
h(x , t) H(t ).x ( ) 
In this case, to update the estimates: 
x = F x (4.2.5) 
k+1ik k kik 
P = FP,F +Q (4.2.6) 
k+1ik k kik k k 
  
  
4.2.1 Iterated Extended Kalman Filter (IEKF) 
The original Kalman filter deals only with linear 
models. In order to use this approach to non-linear 
discrete models Taylor linearization is introduced. In 
the next steps the equations used to get a state 
estimate using IEKF are presented without further 
developments. 
The IEKF approach is based on an iterator which is 
analyzed for each iteration in order to verify the 
convergence. This iterator is given by: 
^ ^ 
T ud ent K omi x h(nt,) z M nlt nD 
(4.2.7) 
where: 
Ron is the Kalman gain matrix at t using 
estimates for the state vector given by mi. Kalman gain 
is expressed by: 
T T -1 
K = 
k;mi P rk-1 Mg i Mini kie Mgr" R. ) (4.2.8) 
M, n is the partial derivatives matrix of 
function h with respect to the elements of the state 
vector: 
8h (x,t) 
M - = ———— (4.2.9) 
kix 
k ox 
m 
P ur is the predicted covariance matrix, which 
is obtained based on an update of the covariance matrix 
computed at op 
Xie is the predicted state at to based on 
measurements at t 1 
state transition function. 
computed using the  linearized 
n, is the iterator, which is an estimate for X it 
at t.c At the first iteration d 7 X tl: which is 
the predicted state estimate at t based on 
measurements taken at t The result of the first 
k-1 
iteration, m2, is used in the third iteration and this 
process is carried out until there is no further 
improvement in mi. Thus, the final estimate for X ik 
is given by the last iterator mi. 
Since the state estimate has  converged the 
filtered covariance matrix can be computed and it is 
given by: 
P = (I- K M)P 
kik k k kik-1 
(-KM)!« KRK! (4240 
k Kk k k k 
When multiples uncorrelated observations are 
available at instant te it is possible to compute 
recursively the estimates for the state at instant te: 
This is accomplished introducing one observation at 
time and supposing that F = I and F = Q = 0; that is, 
the state remains unchanged at t and its estimate and 
covariance matrix are recursively improved for each new 
observation introduced. 
5. FILTERING BASED CALIBRATION AND OBJECT LOCATION 
5.1 Introduction 
The measurement model presented in Section 2.3 can 
be used in a filtering-based approach in order to reach 
a more accurate and flexible method of camera 
calibration and object location. For this purpose model 
equations must state explicitly the relations between 
straight lines parameters in image and object spaces 
and the state vector defined by camera (or object) 
orientation and position variables.