Journal of Mathematical Imaging and Vision 13, 205–228, 2000 c 2000 Kluwer Academic Publishers. Manufactured in The Netherlands. °

The Motor Extended Kalman Filter: A Geometric Approach for Rigid Motion Estimation EDUARDO BAYRO-CORROCHANO∗ Centro de Investigaci´on en Matem´aticas, A.C., Apartado Postal 402, 36000-Guanajuato, Gto, Mexico [email protected]

YIWEN ZHANG Computer Science Institute, Christian Albrechts University, Preußerstraße 1-9, 24105, Kiel, Germany

Abstract. In this paper the motor algebra for linearizing the 3D Euclidean motion of lines is used as the oretical basis for the development of a novel extended Kalman filter called the motor extended Kalman filter (MEKF). Due to its nature the MEKF can be used as online approach as opposed to batch SVD methods. The MEKF does not encounter singularities when computing the Kalman gain and it can estimate simultaneously the translation and rotation transformations. Many algorithms in the literature compute the translation and rotation transformations separately. The experimental part demonstrates that the motor extended Kalman filter is an useful approach for estimation of dynamic motion problems. We compare the MEKF with an analytical method using simulated data. We present also an application using real images of a visual guided robot manipulator; the aim of this experiment is to demonstrate how we can use the online MEKF algorithm. After the system has been calibrated, the MEKF estimates accurately the relative position of the end-effector and a 3D reference line. We believe that future vision systems being reliably calibrated will certainly make great use of the MEKF algorithm. Keywords: computer vision, Clifford algebra, geometric algebra, kinematics, dynamics, rotors; motors, screws, Kalman filter techniques, extended Kalman filter, visual robotics

1.

Introduction

The registration of the motion of a moving object or the computation of the motion between measurement frames in front of an observer is fundamental for various tasks in visual robotics, such as camera calibration, hand-eye calibration, tracking, object collision avoidance and surveillance. The most basic of the 3D geometric primitives of the visual space for motion computation are points (corners). These local features are sensitive to noise and quantization errors that jeopardize the motion estimation. Alternatively, the use of lines (edges) or global features such as planes or surfaces makes the motion estimation process more robust, however, the computational cost is increased especially in the case of planes or surfaces. Since an artificial ob∗ To

whom correspondence should be addressed.

server relies on image sequences, only the projected motion of points, lines, conics, curves or surfaces is distinguished. Using this information we can then compute the actual motion of the object. In this work we are interested in the estimation of the actual rigid motion of an object or more generally the motion between the coordinate axis of the observation frames using 3D observation, as depicted in Fig. 1. We can describe the position and orientation of the coordinate frame Bi relative to the frame A by using a state vector Xi . This state vector satisfies a dynamic model known as the plant model Xi = Φi/i−1 (Xi−1 , W i ),

(1)

where W i denotes random noise with zero mean, normally distributed statistics and with known statistics. The measurement of a line of a scene Li is corrupted

206

Figure 1.

Bayro-Corrochano and Zhan

Coordinate frames for observation of rigid motions.

by noise with known statistics, zero mean, normally distributed and uncorrelated with respect to W i . The relationship between the measurements and the state is given by the following measurement function model f (L0 , Li , Xi , V 0 , V i ) = 0.

(2)

In this noisy scenario the task we have is to find the best estimate of the state variable vector Xˆ i . In the literature we distinguish two main groups: the least squares solutions or batch and the recursive methods [20]. The key characteristic of these methods is whether they compute the translation and rotation transformations simultaneously or decoupled. Arun et al. use point sets by means of least-squares fitting [1] to estimate the rotation and translation separately. In contrast Bayro-Corrochano and Daniilidis use the motion of lines to estimate the motion displacement components simultaneously [6, 10] for solving the hand-eye problem. Zhang and Faugeras, using Pl¨ucker line sets, propose an analytical solution in terms of least-squares to estimate the motion displacement separately [22]. All these approaches are used for static motion estimation. The application of the Kalman filter as a recursive minimum variance estimator has been popular since the sixties [16, 17, 21]. In order to estimate dynamic

motion parameters, the authors used the Kalman filter together with different types of state variable representations. For instance, Bar-Itzhack et al. used point sets for the quaternion extended Kalman filter to estimate the dynamic rotation [3], and Zhang and Faugeras, used line segments with their midpoints to estimate all dynamic motion parameters with a standard extended Kalman filter [22]. Recently, Azarbayejani and Pentland [2] applied the extended Kalman filter for the estimation of motion and structure using relative orientation constraints in terms of quaternions. In this paper, we present the development of a novel extended Kalman filter in the geometric algebra framework. This recursive filter has the virtue of estimating simultaneously the translation and rotation components. This characteristic has not been achieved with the Kalman filter before because the authors could not overcome the singularities when dealing with Pl¨ucker lines. The key idea for the filter design is to work in the 4D geometric algebra, called motor algebra, and to represent the motion of the measurement frames as a motion of lines. The paper shows that the motor extended Kalman filter is an attractive estimation approach, particularly in case of dynamic motion problems. The paper is organized as follows: Section two outlines the geometric algebra. Section three introduces briefly the 4D motor algebra. Section four is devoted to

Motor Extended Kalman Filter

the linear modeling of the 3D motion of lines using the motor algebra. Section five gives a brief introduction to Kalman filter techniques required for the understanding of the rotor Kalman filter. Section six describes the motor extended Kalman filter as a natural generalization. Section seven introduces briefly the analytical method for the recovering of the 3D motion parameters using line observations. In section eight, we compare the MEKF algorithm and an analytical method using simulated data of the screw motion of an object. Then we apply the MEKF algorithm to estimate the relative motion of a robot manipulator which is guided using a stereo vision system. Finally, section nine is dedicated to discussion and conclusions. In this paper, the signature of a geometric algebra Gn will be clearly specified by G p,q,r where p, q and r stand for the numbers of basis vectors which square to +1, −1 and 0 respectively. An even subalgebra will be specified by G+p,q,r . We denote scalars with lower case, matrices with upper case, vectors in 3D with bold lower case and vectors in 4D with bold upper case. 2.

Geometric Algebra

Clifford algebra is a coordinate-free approach to geometry based on the algebras of Grassmann [11] and Clifford [9]. The approach to Clifford algebra we adopt here was pioneered in the 1960’s by David Hestenes [12] and later, with Garret Sobczyk, developed into a unified language for mathematics and physics [13, 14]. Some preliminary applications of geometric algebra to the field of computer vision and neural computing have already been given [5, 7]. 2.1.

207

symmetric and antisymmetric parts ab = a · b + a ∧ b,

(3)

where the inner product a · b and the outer product a ∧ b are defined by 1 (ab + ba) 2 1 a ∧ b = (ab − ba). 2 a·b =

(4) (5)

The inner product of two vectors is the standard scalar or dot product and produces a scalar. The outer or wedge product of two vectors is a new quantity which we call a bivector. We think of a bivector as an oriented area in the plane containing a and b, formed by sweeping a along b, see Fig. 2(a). The bivector b ∧ a has the opposite orientation and is antisymmetric as given in Eq. (5). The outer product is immediately generalizable to higher dimensions. The outer product of k vectors is a k-vector or k-blade, and is said to have grade k, see Fig. 2(b). A multivector (a linear combination of objects of different grades) is said to be homogeneous if it contains terms of only a single grade. In a space of 3 dimensions we can construct a trivector a ∧ b ∧ c, but no 4-vectors exist since there is no possibility of sweeping the volume element a ∧ b ∧ c over a 4th dimension. The highest grade element in a space is called the pseudoscalar. The unit pseudoscalar is denoted by I and is crucial when discussing duality.

Basic Definitions 2.2.

Let Gn denote the geometric algebra of n-dimensions. The geometric or Clifford product of two vectors a and b is written ab and can be expressed as a sum of its

Figure 2.

The Geometric Algebra of n-D Space

In an n-dimensional space we can introduce an orthonormal basis of vectors {σi } i = 1, . . . , n, such that

(a) The directed area, or bivector, a ∧ b. (b) The oriented volume, or trivector, a ∧ b ∧ c.

208

Bayro-Corrochano and Zhan

σi · σ j = δij . This leads to a basis for the entire algebra: 1,

{σi },

{σi ∧ σ j },

{σi ∧ σ j ∧ σk }, . . . ,

σ1 ∧ σ2 ∧ · · · ∧ σn .

the symbol I ; noting that this is not the uninterpreted commutative scalar imaginary j used in quantum mechanics and engineering.

(6) 2.4.

Note that the basis vectors are not represented by bold symbols. Any multivector can be expressed in terms of this basis. In this paper a geometric algebra Gn is of the form G p,q,r , where p, q, and r stand for the number of basis vectors which square to 1, −1 and 0, respectively, where n = p + q + r . Its even subalgebra will be denoted by G+p,q,r . In the n-D space there are multivectors of grade 0 (scalars), grade 1 (vectors), grade 2 (bivectors), grade 3 (trivectors), etc. . . up to grade n. Any two such multivectors can be multiplied using the geometric product. Consider two multivectors Ar and Bs of grades r and s respectively. The geometric product of Ar and Bs can be written as Ar Bs = hABir +s + hABir +s−2 + · · · + hABi|r −s| (7) where hMit is used to denote the t-grade part of multivector M, e.g. consider the geometric product of two vectors ab = habi0 + habi2 = a · b + a ∧ b. As simple illustration the geometric product of A = 5σ3 + 3σ1 σ2 and b = 9σ2 + 7σ 3 is Ab = 35(σ3 )2 + 27σ1 (σ2 )2 + 45σ3 σ2 + 21σ1 σ2 σ3 = 35 + 27σ1 − 45σ2 σ3 + 21I,

(8)

note that σi σi = (σi )2 = σi · σi = 1 and σi σ j = σi ∧ σ j , where the geometric product or equal unit basis vectors equals 1 and of different ones equals to their wedge, which for simple notation can be omitted. 2.3.

The Geometric Algebra of 3-D Space

The basis for the geometric algebra G3,0,0 , of the 3-D space has 23 = 8 elements and is given by: 1 , {σ1 , σ2 , σ3 }, {σ1 σ2 , σ2 σ3 , σ3 σ1 }, |{z} {z } | {z } | scalar

vectors

bivectors

{σ1 σ2 σ3 } ≡ I . {z } |

(9)

trivector

It can easily be verified that the trivector or pseudoscalar σ1 σ2 σ3 squares to −1 and commutes with all multivectors in the 3-D algebra. We therefore give it

Rotors

Multiplication of the three basis vectors σ1 , σ2 , and σ3 by I results in the three basis bivectors σ1 σ2 = I σ3 , σ2 σ3 = I σ1 and σ3 σ1 = I σ2 . These simple bivectors rotate vectors in their own plane by 90◦ , e.g. (σ1 σ2 )σ2 = σ1 , (σ2 σ3 )σ2 = −σ3 etc. Identifying the i, j, k of the quaternion algebra with I σ1 , −I σ2 , I σ3 , the famous Hamilton relations i2 = j2 = k2 = ijk = −1 can be recovered. Since the i, j, k are bivectors, it comes as no surprise that they represent 90◦ rotations in orthogonal directions and provide a well-suited system for the representation of general 3D rotations, see Fig. 3. In geometric algebra a rotor (short name for rotator), R, is an even-grade element of the algebra which satisfies RR˜ = 1, where R˜ stands for the conjugate of R. If A = {a0 , a1 , a2 , a3 } ∈ G3,0,0 represents a unit quaternion, then the rotor which performs the same rotation is simply given by R = a0 + a1 I σ1 − a2 I σ2 + a3 I σ3 = a0 + a1 σ23 − a2 σ31 + a3 σ12 . |{z} | {z } scalar

(10)

bivectors

The quaternion algebra is therefore seen to be a subset of the geometric algebra of 3-space. The conjugated of a rotor is given by R˜ = a0 − a1 σ23 + a2 σ31 − a3 σ12 .

(11)

A rotation can be performed by a pair of reflections, see Fig. 3. It can easily be shown that the result of reflecting a vector a in the plane perpendicular to a unit vector n is a⊥ − ak = a0 = −nan−1 , where a⊥ and ak respectively denote projections of a perpendicular and parallel to n. Thus, a reflection of a in the plane perpendicular to n, followed by a reflection in the plane perpendicular to another unit vector m gives the new vector ˜ Usb = −m(−nan−1 )m−1 = (mn)a(mn)−1 = RaR. ing the geometric product, we can show that the rotor R of Eq. (10) is a multivector consisting of both a scalar part and a bivector part, i.e. R = mn = m · n + m ∧ n. These components correspond to the scalar and vector parts of an equivalent unit quaternion in G3,0,0 . Considering the scalar and the bivector parts, we can further

Motor Extended Kalman Filter

Figure 3.

The rotor in the 3D space formed by a pair of reflections.

write the Euler representation of a rotor as follows θ

R = en 2 = cos

θ θ + n sin , 2 2

pseudoscalars, 1 , γ2 γ3 , γ3 γ1 , γ1 γ2 , γ4 γ1 , γ4 γ2 , γ4 γ3 , |{z} | {z }

(12)

where the rotation axis n = n 1 σ2 σ3 + n 2 σ3 σ1 + n 3 σ1 σ2 is spanned by the bivector basis. The transformation in terms of a rotor a 7→ RaR˜ = b is a very general way of handling rotations; it works for multivectors of any grade and in spaces of any dimension, in contrast to quaternion calculus. Rotors combine in a straightforward manner, i.e. a rotor R1 followed by a rotor R2 is equivalent to a total rotor R where R = R1 R1 .

3.

209

The Motor Algebra

Clifford introduced the motors with the name biquaternions [8, 21]. Motor is the abbreviation of “moment and vector”. Motors are the dual numbers for 3D kinematics with the necessary condition of I 2 = 0. They can be found in the special 4D even subalgebra of G3,0,1 or motor algebra. This even subalgebra denoted by G+ 3,0,1 is spanned by a basis of scalars, bivectors and

scalar

6 bivectors

I |{z}

.

(13)

unitpseudoscalar

Note that the bivectors in the basis correspond to the same basis for spanning 3D lines. Also note that the dual of a scalar is the pseudoscalar P and the duals of the first three basis bivectors are the next three, for example the dual of γ2 γ3 is I γ2 γ3 or γ4 γ1 . 3.1.

Motors, Rotors and Translators in G+ 3,0,1

Since a rigid motion consists of a rotation and a translation, it should be possible to express a motor in terms of individual operators like rotors and translators. The motor action can be described basically in terms of two steps: rotate one axis until its direction is parallel to another axis then shift it to overlap the another one, see Fig. 4. Note that the indicated vectors in the figure will be represented in next sections as bivectors. Let us now express this procedure algebraically. First, let us consider a simple rotor in its Euler

210

Figure 4.

Bayro-Corrochano and Zhan

Screw motion of an object about the line axis l with ts longitudinal displacement by d and rotation Rs with angle θ .

representation for a rotation with angle θ. The rotor of a screw motion, should be represented in terms of a screw axis line as follows Rs = a0 + as n + I as n ∧ tc = ac + as (n + I m) µ ¶ µ ¶ θ θ + sin (n + I m) = cos 2 2 µ ¶ µ ¶ θ θ + sin l. (14) = cos 2 2 Note that the line is expressed using a unit bivector for direction n and dual bivector for the momentum m = n ∧ tc . The motor is defined by sliding along the rotation axis line l the distance ts = dn. Since a motor is applied from the left and its conjugated from the right we use the half of ts when we define the motor µ

¶

ts (a0 + a + I a ∧ tc ) M = Ts Rs = 1 + I 2 ¶ µ dn (ac + as n + I as n ∧ tc ) = 1+ I 2 d d = ac + as n + I as n ∧ tc + I ac n − I as nn 2 ¶ 2 ¶ µ µ d d (n + I n ∧ tc ) = a c − I as + a s + I ac 2 2 ¶ µ ¶ µ d d + as + I ac l. (15) = ac − I as 2 2

Note that this expression of the motor makes explicit the unitary screw axis line l. Now let us express a motor as an Euler representation. Substituting the constants ac = cos( θ2 ) and as = sin( θ2 ) in the motor Eq. (15) and using the propertry of scalar functions with dual argument we get µ ¶ ¶ µ µ ¶ θ d θ M = Ts Rs = cos − I sin 2 2 2 µ ¶ ¶ µ µ ¶ θ d θ + I cos l + sin 2 2 2 ¶ µ ¶ µ d θ d θ +I + sin +I l. (16) = cos 2 2 2 2 If we want to express the motor using only a rotor and its conjugated given by R˜ = r0 − r1 σ2 σ3 − r2 σ3 σ1 − r3 σ1 σ2 = r0 − r, (17) we proceed as follows µ ¶ ts M = T s Rs = 1 + I Rs 2 ts = Rs + I Rs = Rs + I R0s . 2

(18)

We can now express the bivector ts in terms of the rotors µ ¶ ts R0s R˜ s = Rs R˜ s (19) 2

Motor Extended Kalman Filter

describes the overall displacement, namely

so that ˜ ts = 2R0s R.

¡ ¢¡ ¢ Mc = Ma Mb = Rsa + I R0sa Rsb + I R0sb ¡ ¢ = Rsa Rsb + I Rsa R0sb + R0sa Rsb

(20)

Figure 4 shows that t is a 3D distance vector referred to coordinate system of the rotation axis of an object and tk or ts is a bivector along the motor axis line. The distance t, considered here as a bivector can be computed in terms of the bivectors tc and ts as follows t = t⊥ + tk = t⊥ + ts = (tc − Rs tc R˜ s ) + (t · n)n = (tc − Rs tc R˜ s ) + dn = tc − Rs tc R˜ s + ts = (tc − Rs tc R˜ s ) + (2R0s R˜ s ). 3.2.

211

(21)

= Rsc + I R0sc .

Note that pure rotations combine multiplicatively, whereas the dual parts containing the translation combine additively. Using the Eq. (18), let us express a motor in terms of a scalar, bivector, dual scalar and dual bivector M = Ts Rs = Rs + I R0s = (a0 + a1 γ2 γ3 + a2 γ3 γ2 + a3 γ2 γ1 ) + I (b0 + b1 γ2 γ3 + b2 γ3 γ2 + b3 γ2 γ1 ) = (a0 + a) + I (b0 + b).

Properties of Motors

Mα = αM

(22)

where a ∈ R and M is a unit motor as in the previous sections. The norm of a unit motor M is defined by ¶ ¶ µ µ ts ts ˜ ˜ ˜ ˜ |M| = MM = Ts Rs Rs Ts = 1 + I R s Rs 1 − I 2 2 ts ts = 1 + I − I = 1, (23) 2 2 ˜ is the conjugate of the motor, defined by where M f = R˜ T˜ ˜ = TR M

(24)

and 1 is the identity of the motor multiplication. Now using the Eq. (18), and considering the unit motor magnitude we find two useful properties ˜ = (Rs + I R0s )(R˜ s + I R˜ 0s ) |M| = MM = Rs R˜ s + I (R˜ s R0s + R˜ s Rs ) = 1.

(25)

Rs R˜ s = 1 + R˜ s Rs =

M = (a0 , a) + I (b0 , b),

0

− r · r ) = 0.

(26) (27)

The combination of two rigid motions can be expressed using two motors. The resultant motor

(30)

where each term within the parenthesis consists of a scalar part and a 3D bivector. 4.

Linear Modeling of the Motion of 3D lines using the Motor Algebra

The modeling of the 3-D motion of lines using the motor algebra G+ 3,0,1 takes place in a 4D space where the rotation and translation are operators applied multiplicatively; as a result the 3D general motion becomes linear. Having a linear method we can then compute, for example, the unknown rotation and translation simultaneously in such cases as the hand-eye problem [6] or estimate dynamic motion using the motor extended Kalman filter. In these problems, if we use the 3D geometric algebra G3,0,0 , we are unfortunately compelled to compute the translation decoupled of rotation, increasing therefore the inaccuracy. 4.1.

This requires that

2(r0r00

(29)

We can use another notation to enhance the components of the real and dual parts of the motor as follows

A general motor can be expressed as

R˜ s R0s

(28)

Representation of 3D Lines Using the Motor Algebra

The basis bivectors of the algebra of motors G+ 3,0,1 , spans the line space in 4D. Assume that two points X1 = (X 11 , X 12 , X 13 , 1) and X2 = (X 21 , X 22 , X 23 , 1) in the space of G3,0,1 lying on the hyper-plane X 4 = 1 belong to the line Ld . Note that we use now upper

212

Bayro-Corrochano and Zhan

Figure 5.

(a) The moment and the direction of the line using dual bivector basis. (b) The screw motion of a line.

letters for points in the 4D space to differentiate them from the points of the 3D space in the G3,0,0 . The line belongs to G+ 3,0,1 and therefore it can be defined simply as an outer product of these points, i.e. Ld = X1 ∧ X2 = (X 12 X 23 − X 13 X 22 )γ2 γ3

The real part can be seen as the direction of this line, and the dual part as the moment of the plane where the line lies. This plane crosses the origin and it is described by the outer product between n and any vector p touching the line, i.e. Ld = n + n ∧ p = n + I m.

+ (X 13 X 21 − X 11 X 23 )γ3 γ1 + (X 11 X 22 − X 12 X 21 )γ1 γ2 + (X 21 − X 11 )γ4 γ1 + (X 22 − X 12 )γ4 γ2 + (X 23 − X 13 )γ4 γ3 .

(31)

Since this equation consists only of bivectors, it can be expressed straightforwardly in terms of the bivector basis, namely Ld = (L 23 γ2 γ3 + L 31 γ3 γ1 + L 12 γ1 γ2 ) + (L 41 γ4 γ1 + L 42 γ4 γ2 + L 43 γ4 γ3 )

Note that the term n × p is orthogonal to the bivector n, so we can use the dual bivector representation for the moment m. The representation of the line according the Eq. (34) is given in Fig. 5(a). This line representation using dual numbers is easier to understand and to manipulate algebraically and it is fully equivalent to the one in terms of Pl¨ucker coordinates. Using the notation with brackets the line equation reads

= (L 23 γ2 γ3 + L 31 γ3 γ1 + L 12 γ1 γ2 )

Ld = (0, n) + I (0, m).

+ I (L 41 γ2 γ3 + L 42 γ3 γ1 + L 43 γ1 γ2 ), (32) where γ4 γ1 = I γ2 γ3 , γ4 γ2 = I γ3 γ1 and γ4 γ3 = I γ1 γ2 . To agree with the conventional line representation, we interchange the coefficients of the bivectors and we express three of them as the dual part as follows Ld = (L 41 γ2 γ3 + L 42 γ3 γ1 + L 43 γ1 γ2 ) + (L 23 γ2 γ3 + L 31 γ3 γ1 + L 12 γ1 γ2 ), (33) Note that this is equivalent to the line expression using Pl¨ucker coordinates.

(34)

(35)

where the n and m are spanned with a 3D bivector basis. Note that the first element of within the parenthesis corresponds to the scalar and the second to the bivector part. 4.2.

Modeling the Motion of Lines Using the Motor Algebra

The motion of a 3D line, or the screw motion of a line can also be seen as a rotation of the line about the axis

Motor Extended Kalman Filter

line Ls , followed by its translation along this axis line, as depicted in Fig. 5(b). Note that in the figure the line Ls is shifted from the origin in tc . In Section 3.1 we explained in detail how a motor implements the screw motion of an object about a line axis. Considering now the 3D line as a geometric object, and using the line Eq. (34), we can express the motion of a 3D line as follows ˜ L0 = n0 + I m0 = M(L)M = Ts Rs (n + I m)R˜ s T˜ s .

µ

¶ ¶ µ ts ts ˜ Rs (n + I m)Rs 1 − I L = 1+ I 2 2 ¶µ ¶ µ ts ts ˜ ˜ ˜ Rs nRs + I Rs mRs − I Rs nRs = 1+ I 2 2 µ ¶ t t s s ˜ ˜ ˜ ˜ = Rs nRs + I −Rs nRs + Rs nRs + Rs mRs 2 2 0 (37) = Rs nR˜ s + I (Rs nR˜ s + R0s nR˜ s + Rs mR˜ s ) 0

where U = (u 0 u 1 u 2 u 3 )T , V = (v0 v1 v2 v3 )T and  u 0 −u 1 u  1 u0 U Rl =  u 2 −u 3 u3 u2  v0 −v1 v  1 v0 V Rr =  v2 v3 v3 −v2

(36)

This equation can be expressed purely in terms of rotors as follows

−u 2 u3 u0 −u 1 −v2 −v3 v0 v1

 −u 3 −u 2   , u1  u0

 −v3 v2   . −v1 

4.3.

We call U Rl “left-multiplication matrix of rotor U” and V Rr “right-multiplication matrix of rotor V”. The product of S = U + I U0 and T = V + I V 0 in geometric algebra G+ 3,0,1 gives Q = ST = (U + I U0 )(V + I V 0 ) = UV + I (UV 0 + U0 V),

(41)

where U, U0 , V and V 0 are all expressed in the form of rotors. Multiplication of these two motors in terms of matrices (42)

where S = (u 0

u1

u2

u3

u 00

u 01

u 02

u 03 )T,

T = (v0 v1 v2 v3 v00 v10 v20 v30 )T, (43) µ µ ¶ ¶ U Rl 04×4 V Rr 04×4 , T Mr = , S Ml = 0 0 U Rl U Rl V Rr V Rr

Representation of the Line Motion Model in Linear Algebra

The line motion model presented in the last section uses the geometric algebra G+ 3,0,1 . Because the EKF computer algorithm is implemented using techniques of linear algebra, we should also formulate the line mo˜ in the frame of linear algebra. tion model L0 = MLM Let us start using rotor relations. The multiplication of two rotors U and V in geometric algebra G+ 3,0,1 reads

We call S Ml “left-multiplication matrix of motor S” and T Mr “right-multiplication matrix of motor T”. To convert the line motion model of Eq. (36) to matrix algebra we can handle the real and dual components n, m, n0 and m0 of the lines L and L0 as rotors with zero scalar. By right multiplication of both sides of Eq. (36) by M we get L0 M − ML = 0.

W = UV = (u 0 + u)(v0 + v) = u 0 v0 + u · v + u 0 v + v0 u + u ∧ v.

(40)

v0

Q = S Ml T = T Mr S, This equation is very useful for estimating the rotation and translation simultaneously as in the case of hand-eye calibration [6] or for the algorithm for the motor extended Kalman filter.

213

(38)

(44)

This results in the following linear motion equation

Multiplication of these two rotors in linear algebra is

(L0Ml − L Mr )M = A M M = 0,

W = U Rl V = V Rr U,

this matrix representation of the line motion was suggested in [10]. The constraints of Eqs. (26) and (27),

(39)

(45)

214

Bayro-Corrochano and Zhan

respectively, now are RT R = 1, 0

R R = 0, T

(46) (47)

with R = (r0 r1 r2 r3 )T , R0 = (r00 r10 r20 r30 )T and M = R + I R0 . These properties will be used for the implementation of the MEKF algorithm in the subsection 6.2. 5.

Let us describe the Eq. (1) of a dynamical system by a linear difference state equation as follows (48)

The state of the system at ti is given by the ndimensional vector Xi . The term Φi/i−1 is an n × n transition matrix and Wi is the random error with the known first- and second-order characteristics E[Wi ] = 0, i = 0, 1, . . . ¤ E Wi W Tj = Qi δij £

(49) (50)

where δij is the Kronecker delta function. The matrix Qi is assumed to be nonnegative-definite.

(52) (53)

where the matrix Ci is assumed to be nonnegativedefinite [21]. Further, assume that the random processes Wi and Vi are uncorrelated, i.e. for each i, j ¤ £ E Wi V Tj = O,

(54)

where O is the zero matrix. Given the preceding models (48) and (51), we shall determine an estimate Xˆ i of the state at ti as a linear combination of an estimate Xˆ i−1 at ti−1 and the data Zi measured at time ti . By defining an unknown (n × m) gain matrix Ki , the estimate Xˆ i is given by Xˆ i = Φi/i−1 Xˆ i−1 + Ki [Zi − Hi Φi/i−1 Xˆ i−1 ] (55) The matrix Ki is determined so that the estimate has the minimal variance. That is, the Xˆ i is chosen to minimize its mean squared error E MIN = {E[(Xˆ i − Xi )T (Xˆ i − Xi )]}MIN .

The Kalman Filter

(51)

where Hi is a known m × n observation matrix, and the vector Vi a random error with the known statistics E[Vi ] = 0, i = 0, 1, . . . ¤ E Vi V Tj = Ci δij ,

Recursive Estimation using Kalman Filter Techniques

Xi = Φi/i−1 Xi−1 + Wi .

Zi = Hi Xi + Vi ,

£

The Kalman filter is a linear recursive algorithm, that is unbiased and of minimum variance. It is used to estimate optimally the unknown state of a linear dynamic system using noisy data which is taken at discrete real-time intervals. The extended Kalman filter (EKF) approach modifies the standard Kalman filter (used for linear systems) in order to treat noisy nonlinear systems. It starts with an initial guess, then updates continually the predicted state with new measurements. Unfortunately, if the disturbances are so large that the linearization is inadequate to describe the system, the filter will not converge to a reasonable estimate. First, we give a brief outline of the Kalman filter and of the extended Kalman filter, using this background we will explain thereafter the rotor and motor extended Kalman filters. For a more complete introduction the reader should refer to [21]. 5.1.

Suppose that at each time ti there is available an mdimensional vector of measurement Zi that is linearly related to the state and which is corrupted by the additive noise Vi . This is the so called observation equation

(56)

Equation (56) is equivalent to the minimization of the trace of state error covariance matrix Pi E MIN = {trace Pi }MIN = {trace E[(Xˆ i − Xi )(Xˆ i − Xi )T ]}MIN . (57) By substituting (51) in (55), and then substituting the new (55) and (48) in (57), it can be shown that the trace of matrix Pi will be minimized by choosing the following optimal gain matrix Ki , ¡ ¢−1 Ki = Pi/i−1 HiT Hi Pi/i−1 HiT + Ci ,

(58)

where Pi/i−1 is the error covariance matrix T + Qi . Pi/i−1 = Φi/i−1 Pi Φi/i−1

(59)

Motor Extended Kalman Filter

of the predicted state Xˆ i/i−1 = Φi/i−1 Xˆ i .

(60)

With this optimal gain matrix Ki , the matrix Pi reduces to

In computer vision the measurement model is usually found to be described by a nonlinear observation equation fi (Z0,i , Xi ) = 0, where the parameter Z0,i is the accurate measurement. In practice, such a measurement is affected by random errors. We assume that the measurement system is disturbed by additive white noise

Pi = Pi/i−1 − Ki Hi Pi/i−1 = (I − Ki Hi )Pi/i−1 .

lim Ki = Hi−1 .

Ci →O

(62)

On the other hand, as the estimated state error covariance Pi approaches zero, the gain Ki weights the residual less heavily, lim Ki = O.

Pi →O

(63)

Another way of thinking about the weighting the Kalman filter by Ki is that as the measurement error covariance matrix Ci approaches zero, the actual measurement Zi is “trusted” more and more, while the predicted state Φi/i−1 Xˆ i is trusted less and less. On the other hand, as the estimated state error covariance Pi approaches zero the actual measurement Zi is trusted less and less, while the predicted state Φi/i−1 Xˆ i (the dynamic model) is trusted more and more. 5.2.

Zi = Z0,i + Vi ,

(61)

Equations (55), (59), (58) and (61) constitute the Kalman filter equations for the model of the system (48) and that of the measurement (51), respectively. From (58), we see that as the measurement error covariance matrix Ci approaches zero, the gain matrix Ki weights the residual more heavily,

The Extended Kalman Filter

As described previously, the Kalman filter addresses the general problem of trying to estimate the state Xi of a discrete-time controlled process that is governed by a linear stochastic difference equation. But what happens if the process and (or) the relation between the measurement and the state is non-linear? Some of the most interesting and successful applications of Kalman filtering are concerned with such situations. A Kalman filter that linearizes about the current predicted state Xˆ i/i−1 and measurement Zi is referred to as an extended Kalman filter or EKF.

215

(64)

where the statistics of noise Vi are given by (52) and (53). To apply the Kalman filter technique, we must expand the nonlinear observation equation into a first order Taylor series about (Zi , Xˆ i/i−1 ), fi (Z0,i , Xi ) ∂fi (Zi , Xˆ i/i−1 ) (Z0,i − Zi ) = fi (Zi , Xˆ i/i−1 ) + ∂Z0,i ∂fi (Zi , Xˆ i/i−1 ) (Xi − Xˆ i/i−1 ) + O2 = 0. (65) + ∂Xi By ignoring the second order term O2 , the linearized measurement Eq. (65) becomes Yi = Hi Xi + Ni ,

(66)

where Yi is the new measurement vector, Ni is the noise vector of the new measurement, and Hi is the linearized transformation matrix. The components of the Eq. (66) are given by ∂fi (Zi , Xˆ i/i−1 ) ˆ Xi/i−1 , Yi = −fi (Zi , Xˆ i/i−1 ) + ∂Xi ∂fi (Zi , Xˆ i/i−1 ) , Hi = ∂Xi ∂fi (Zi , Xˆ i/i−1 ) (Z0,i − Zi ), Ni = ∂Z0,i E[Ni ] = 0, ¤ £ E Ni NiT = Ci/i−1 ∂fi (Zi , Xˆ i/i−1 ) ∂fi (Zi , Xˆ i/i−1 )T = Ci , ∂Z0,i ∂Z0,i where Ci is given by the statistics of measurement (53). This linearized equation (66) is a general form for the nonlinear model. We will use this form for our particular nonlinear measurement model later in Section 6.

216

5.3.

Bayro-Corrochano and Zhan

The Rotor Extended Kalman Filter

+

This subsection describes an EKF algorithm to estimate rotors or quaternions. We call it Rotor Extended Kalman Filter (REKF). In the static case the measurements of points free of error satisfy Ri+1 = Ri , p00,i+1

(67)

= R(Ri+1 )p0,i+1 ,

(68)

where { p0,i } and { p00,i } are sets of points before and after rotation, respectively, and Ri is the rotation quaternion 0 . R(R) is the for the ith pair of points p0,i and p0,i matrix representation of R R(R)  2 r1 + r22 − r32 − r42  =  2(r2 r3 − r1 r4 ) 2(r2 r4 + r1 r3 )

2(r2 r3 + r1 r4 ) r12

− r22

+ r32

2(r2 r4 − r1 r3 )

2(r4 r3 − r1 r2 )



 2(r4 r3 + r1 r2 ) ,

− r42

r12 − r22 − r32 + r42

(69)

0 ∂fi+1 (pi+1 , pi+1 , Rˆ i+1/i ) (p0,i+1 − pi+1 ) ∂p0,i+1

0 ∂fi+1 (pi+1 , pi+1 , Rˆ i+1/i ) (Ri+1 − Rˆ i+1/i ) + O2 ∂Ri+1 = 0, (74)

+

where the second order term O2 can be omitted, and 0 ∂fi+1 ( pi+1 , pi+1 , Rˆ i+1/i ) = 1, ∂p00,i+1

0 , Rˆ i+1/i ) ∂fi+1 ( pi+1 , pi+1 = −R(Rˆ i+1/i ), (76) ∂p0,i+1 0 ∂fi+1 ( pi+1 , pi+1 , Rˆ i+1/i ) ∂R(Rˆ i+1/i )pi+1 = . (77) ∂Ri+1 ∂Ri+1

In order to compute the expression (77), we utilize the following vectors

where r j for j = 1, 2, 3, 4 are the four components of R which satisfies kRk = 1.

pi+1 = p0,i+1 + ni+1

(71)

0 pi+1

(72)

=

p00,i+1

+

pi+1 ∼ = ( p1 p2 p3 ) T , Ri+1 ∼ = (r1 r2 r3 r4 )T Rˆ i+1/i ∼ = (ˆr1 rˆ2 rˆ3 rˆ4 )T ,

(70)

Let us assume that the measurements {pi+1 } and 0 {pi+1 } of {p0,i+1 } and {p00,i+1 } are corrupted by the 0 }, respectively, such that noise {ni+1 } and {ni+1 0 ni+1 .

0 Here the noise vectors {ni+1 } and {ni+1 } are assumed to have zero mean and the known covariance matrices 0 }, respectively. {Ci+1 } and {Ci+1 We rewrite the Eq. (68) as the function fi+1 depending of the variables ( p0,i+1 , p00,i+1 , Ri+1 )

(79) (80)

thus we can write R(Rˆ i+1/i )pi+1  2 rˆ1 + rˆ22 − rˆ32 − rˆ42 2(ˆr2 rˆ3 + rˆ1 rˆ4 )  =  2(ˆr2 rˆ3 − rˆ1 rˆ4 ) rˆ12 − rˆ22 + rˆ32 − rˆ42 

2(ˆr2 rˆ4 + rˆ1 rˆ3 ) 

p1   = ( pˆ 10  p2  p3 i+1

2(ˆr4 rˆ3 − rˆ1 rˆ2 ) pˆ 20

2(ˆr2 rˆ4 − rˆ1 rˆ3 )



 2(ˆr4 rˆ3 + rˆ1 rˆ2 )  rˆ12 − rˆ22 − rˆ32 + rˆ42

0 pˆ 30 )T = pˆi+1/i .

(81)

(73)

0 , Rˆ i+1/i ) in Expanding this equation about ( pi+1 , pi+1 terms of first order Taylor series we get

∂R(Rˆ i+1/i )pi+1 ∂Ri+1

fi+1 (p0,i+1 , p00,i+1 , Ri+1 ) 0 = fi+1 ( pi+1 , pi+1 , Rˆ i+1/i ) 0 ∂fi+1 ( pi+1 , pi+1 , Rˆ i+1/i ) 0 0 ( p0,i+1 − pi+1 ) + 0 ∂p0,i+1

(78)

0 with respect to a The derivative of the vector pˆ i+1/i vector Ri+1 is of the form

fi+1 ( p0,i+1 , p00,i+1 , Ri+1 ) = p00,i+1 − R(Ri+1 )p0,i+1 = 0

(75)

=

0 ∂ pˆ i+1/i

∂Ri+1



∂ pˆ 10  ∂r1  0  ∂ pˆ 2 =  ∂r  1  ∂ pˆ 0 3 ∂r1

∂ pˆ 10 ∂r2 ∂ pˆ 20 ∂r2 ∂ pˆ 30 ∂r2

∂ pˆ 10 ∂r3 ∂ pˆ 20 ∂r3 ∂ pˆ 30 ∂r3

 ∂ pˆ 10 ∂r4   ∂ pˆ 20  . ∂r4   ∂ pˆ 0  3

∂r4

(82)

Motor Extended Kalman Filter

Now defining a 3 × 4 matrix Hi+1/i = − we can write the previous equation as −Hi+1/i

∂R(Rˆ i+1/i )pi+1 = ∂Ri+1  h1 h2 h3  =  h 4 −h 3 h 2 −h 3 −h 4 h 1

∂ R(Rˆ i+1/i )pi+1 ∂Ri+1

0 where Ci+1 and Ci+1 are the known covariance matrices 0 and ni+1 respectively. of the noise ni+1



h4  −h 1 , h2

(83)

h 1 = 2(ˆr1 p1 + rˆ4 p2 − rˆ3 p3 ), h 2 = 2(ˆr2 p1 + rˆ3 p2 + rˆ4 p3 ), h 3 = 2(−ˆr3 p1 + rˆ2 p2 − rˆ1 p3 ), h 4 = 2(−ˆr4 p1 + rˆ1 p2 + rˆ2 p3 ). First replacing (83) in (77) and then substituting it together with (75) (76) in (74) and then omitting the second order terms we get 0 0 0 = pi+1 − R(Rˆ i+1/i )pi+1 + ( p00,i+1 − pi+1 )

− R(Rˆ i+1/i )( p0,i+1 − pi+1 ) − Hi+1/i (Ri+1 − Rˆ i+1/i ).

(84)

0 = Hi+1/i Ri+1 + ( pi+1 − p00,i+1 )

(85)

The components of this equation are now amenable to be identified as the measurement zi+1 and the noise of the measurement ni+1/i as follows 0 − R(Rˆ i+1/i )pi+1 + Hi+1/i Rˆ i+1/i , zi+1 = pi+1

(86) 0 − p00,i+1 ) − R(Rˆ i+1/i )( pi+1 − p0,i+1 ) ni+1/i = ( pi+1

− R(Rˆ i+1/i )ni+1 .

(87)

In terms of these variables we can finally write the first order linearized measurement equation more compactly zi+1 = Hi+1/i Ri+1 + ni+1/i .

∗ Rˆ i+1/i = Rˆ i , ˆ i+1/i = Pi . P

(90) (91)

0 Taking into account the measurements pi+1 , pi+1 and the predicted state Rˆ i+1/i the new measurement zi+1 , Eq. (86) can be straightforwardly computed. Then, according Eqs. (55) and (58) the Kalman gain matrix and the estimate Rˆ i+1 at step i + 1 by EKF are computed respectively as follows

(Hi+1/i Pi+1/i Hi+1/i + Ci+1/i )−1 , (92) Rˆ i+1 = Rˆ i+1/i + Ki+1 (zi+1 − Hi+1/i Rˆ i+1/i ) 0 = Rˆ i+1/i + Ki+1 (pi+1 − R(Rˆ i+1/i ) pi+1 ). (93)

0 pi+1 − R(Rˆ i+1/i )pi+1 + Hi+1/i Rˆ i+1/i

− R(Rˆ i+1/i )( pi+1 − p0,i+1 ).

5.3.1. Rotation Estimation. Now we will describe the procedure of the estimation of a rotation expressed as a the rotor R. At the beginning of the iteration or step i = 0, the initial state Rˆ 0 and an initial estimation error covariance matrix P0 are given. According to the ∗ Eq. (67), given an estimate Rˆ i , and the covariance matrix Pi at step i, it is reasonable to assign the following as a prediction of the next step i + 1

T Ki+1 = Pi+1/i Hi+1/i

This equation can be further rearranged as

=

Here ni+1/i is zero mean noise with covariance given by 0 ˆ i+1/i )Ci+1 RT (Rˆ i+1/i ). (89) Ci+1/i = Ci+1 + R(R

def

where

0 ni+1

217

(88)

Rˆ i+1 must be modified prudently to satisfy the constraint kRk = 1: Rˆ i+1 ∗ Rˆ i+1 = kRˆ i+1 k

(94)

The updating of the error covariance matrix follows according to Pi+1 = (I − Ki+1 Hi+1 )Pi+1/i (I − Ki+1 Hi+1 )T T + Ki+1 Ci+1 Ki+1 .

(95)

Note that Hi+1 and Ci+1 are recalculated using the ∗ current estimate Rˆ i+1 . As far as the implementation of the filter is concerned, the measurement error covariance matrix Ci might be measured prior to the operation of the filter.

218

6.

Bayro-Corrochano and Zhan

The Motor Extended Kalman Filter

The use of the EKF filter in the motor algebra framework gives us the simultaneous estimation of translation and rotation. According to the literature there are only batch methods for the simultaneous estimation of these components [6, 15]. The motor extended Kalman filter (MEKF) turns out to be a natural extension of the rotor extended Kalman filter thanks to the multivector concept of the geometric algebra. First, let us define the noisy motion equation using lines in the motor algebra framework G+ 1,3,0 . The geometric features we will consider for the measurements are 3D lines (L1, L2, . . . , Ln, n ≥ 2) which belong to an object moving in the 3D space, see Fig. 1. The rigid motion parameters between any couple of consecutive time instants (t0 , t1 , t2 , . . . , t N ) are described compactly by the motor Mi . According to the Eq. (36), the motion of any line of the object is modeled by ˜ i. Li = Mi Li−1 M

(97)

we can then express the recursive motion equation of the line in general as follows

where the statistics of Wi is given by (49) and (50). Note that Vi/i−1,Ml means “left-multiplication matrix” of motor Vi/i−1 . 6.1.

Linearization of the Measurement Model

Considering Eq. (45), we can easily see that the relation between the measurement A M and the state M is unfortunately nonlinear; that is why we should linearize it. Assume that the measurement AMi is the true data AM0,i contaminated by measurement noise NA M ,i with zero mean and known covariance matrix CA M ,i AMi = AM0,i + NA M ,i .

(102)

ˆ i/i than Supposing that the predicted state of Mi is M according the Eq. (74) we can define a function f M,i depending of the variables (AM0,i , Mi ) as follows f M,i (AM0,i , Mi ) = AM0,i Mi = 0.

(103)

(98)

Thus, we obtain the ideal dynamic motion model in terms of the motors Mi = Vi/i−1 Mi−1 .

(101)

This equation can be expanded into a first order Taylor ˆ i/i−1 ) series about the predicted state (AMi , M

˜i Li = Mi Li−1 M ˜ i−1 V˜ i/i−1 ). = (Vi/i−1 Mi−1 )Li−1 (M

Mi = Vi/i−1,Ml Mi−1 + Wi ,

(96)

If the change of the parameters of the line in motion between the time instants ti−1 and ti is described in terms of the motor velocity information Vi/i−1 Li = Vi/i−1 Li−1 V˜ i/i−1 ,

Since in real work the relation between Mi−1 and Mi is known only approximately, the real dynamic model of the noisy 3D motion is given by

ˆ i/i−1 ) f M,i (AM0,i , Mi ) = f M,i (AMi , M +

ˆ i/i−1 ) ∂f M,i (AMi , M ˆ i/i−1 ) (Mi − M ∂Mi

(99) + (AM0,i − AMi )

For example, suppose the motion is a screw motion with rotation of constant angular velocity ω about an axis of known line (Ls = r¯ + I tc ∧ r¯) and with constant translation velocity vs along the axis line. If the data sampling is done at equidistant time interval, then the instant times can be represented by integers, so the motor equation reads Vi/i−1 = V = (1 + I vs /2)(cos(ω/2) + sin(ω/2)Ls ). (100)

ˆ i/i−1 ) ∂f M,i (AMi , M ∂AM0,i

+ O2 = 0.

(104)

Now calling the components ˆ i/i−1 ) ∂f M,i (AMi , M = AMi , ∂Mi

(105)

ˆ i/i−1 ) ∂f M,i (AMi , M ˆ i/i−1 , =M ∂AM0,i

(106)

Motor Extended Kalman Filter

omitting the second order terms O2 and considering the Eq. (102) for AMi+1 we get

and the error covariance matrix for the i-step is updated as Pi = Pi/i−1 − Ki Hi Pi/i−1 = (I − Ki Hi )Pi/i−1 .

ˆ i/i−1 ) ˆ i/i−1 + AMi (Mi − M AMi M

(113)

ˆ i/i−1 = AMi M ˆ i/i−1 + (AM0,i − AMi )M ˆ i/i−1 ) − NAM,i+1 M ˆ i/i−1 = 0. (107) + AMi (Mi − M or

Now Mi∗ consists of two 4-dimensional vectors and R0 i∗ which must be varied to fulfill the constraints (46) and (47). In the case of the constraint (46), the modifications can be done simply by

Ri∗

R0 i∗ . Rˆ i = kR0i∗ k

ˆ i/i−1 −AMi Mi + NA M,i M ˆ i/i−1 − AMi M ˆ i/i−1 = 0 = AMi M

(108)

As a result we can claim that the measurement equation for the MEKF at step i is given by ˆ i/i−1 Zi = −AMi Mi + NA M,i M = Hi Mi + N Z ,i = 0,

(109)

where we call Hi = −A Mi and N Z ,i = NA M,i+1 ˆ i/i−1 . The covariance matrix of N Z ,i is Ci . M 6.2.

Enforcing a Geometric Constraint

In order to estimate the motor state, we assume first ˆ 1/0 that at the beginning or step i = 0 the initial state M and the initial error covariance matrix of the estimate P1/0 are given. Now, according to Eqs. ((55), (59), (58) and (61)), the estimation equation of the motor state is given by

R0 R = 0 T

cos(ϕ) =

R0i∗T Ri∗ kR0i∗ k · kRi∗ k

where the optimal Kalman gain matrix Ki is computed according to the formula (111)

where (112)

(116)

which can be simplified by using Eq. (114) and the unit rotor Rˆ i as follows

ˆ i−1 + Ki (Zi − Hi Vi/i−1,Ml M ˆ i−1 ) = Vi/i−1,Ml M ¡ ∗T ¢T , (110) = Ri∗T R0i

T Pi/i−1 = Φi/i−1 Pi Φi/i−1 + Qi

(115)

tells us that R must be orthogonal to the dual rotor R0 and this is valid up to a scalar, see the role of this scalar in the Eq. (120) below. Unfortunately in the practice the rotor estimate Ri∗ is usually not orthogonal to the estimated dual rotor R0i∗ . The Fig. 6 suggests clearly how to enforce this geometric constraint in order to modify the orientation of R0i∗ . In order to do so first we consider the cosine of the angle ϕ between estimates Ri∗ and R0 i∗

ˆ i−1 + Ki (Zi − Hi Φi/i−1 M ˆ i−1 ) = Φi/i−1 M

¡ ¢−1 Ki = Pi/i−1 HiT Hi Pi/i−1 HiT + Ci .

(114)

However, it is not that simple to satisfy the constraint (47). The constraint

cos(ϕ) = Mi∗

219

Figure 6.

0 Constraint of Rˆ Rˆ = 0.

R0i∗T Rˆ i . kR0i∗ k

(117)

220

Bayro-Corrochano and Zhan

Then we consider also the orthogonal variation ¡ ∗T ¢ 0∗ 0∗ δ Rˆ i = kRˆ i k cos(ϕ)Rˆ i = R0i Rˆ i Ri .

(118)

It is then straightforward to build the following difference relation ¡ ∗ ¡ ∗T ¢ ¢ 0 Rˆ i = k R0i − R0i Rˆ i Rˆ i .

(119)

Note that this equation is valid up to a scalar k which can be defined as

In general, during the MEKF cycle of Fig. 7(a), the updating uses a prediction and a corrected input measurement to actualize a new estimate which in turn will be modified using a geometric constraint. This cycle continues for each new measurement on and on. After few iterations the MEKF should have stabilized to the estimated proper states. The Kalman gain Ki can be calculated before the actual estimation is carried out and it does not depend at all on the measurement Zi . The computation cycle for Ki illustrated in Fig. 7(b) would proceed as follows

0

αkRˆ i k k= ¡ ¢ ∗ , kR0 i∗ − R0i∗T Rˆ i Rˆ i k · kRˆ i k

(120)

Step 1

given Pi−1/i−1 = Pi , Qi and Φi/i−1 then Pi/i−1 is computed using the Eq. (112).

2

Pi/i−1 , Hi and Ci are substituted in Eq. (111) to obtain Ki , which will be used in step 3 of the MEKF algorithm.

3

Pi/i−1 , Ki , and Hi are substituted in Eq. (113) to determine Pi , which is stored until the time of the next measurement, when the cycle is repeated

therefore 0 Rˆ i

¡

¡ ¢ ¢ 0 R0i∗ − R0i∗T Rˆ i Rˆ i kRˆ i k =α ¡ ¢ 0∗ kR0i∗ − R0i∗T Rˆ i Rˆ i k kRˆ k

(121)

i

where the scalar α can be find out thereafter by filter tuning. An illustration of the effect of the tuning will be presented in the experimental part. 6.3.

Operation of the MEKF Algorithm

The processing of information by the MEKF filter can be explained very simply by considering the block diagrams presented in Fig. 7(a) and (b).

Figure 7.

Procedure

Next the MEKF algorithm illustrated in Fig. 8 will be explained. For the initialization of the MEKF we can ˆ i−1 use for the initial time instant i known values of M and Pi−1 or if we do not known them simply take the ˆ i−1 = [10000000]T and Pi−1 = I8×8 . trivial values M After the initialization the MEKF seeks to determine ˆ i for some future time instant. The computation proM cedure of the MEKF would proceed as follows

(a) Estimation and up-dating cycles (b) Kalman gain computation.

Motor Extended Kalman Filter

Step

Action

1

Prediction

2

Estimation

3

4

Correction

Procedure ˆ i−1 is propagated forward The estimated M by premultiplying it by the discrete system model matrix Φi/i−1 . This ˆ i/i−1 = gives the predicted estimate M ˆ i−1 . Then the measurement Φi/i−1 M model Hi = −AMi is linearized, see Eq. (45). ˆ i/i−1 is premultiplied by Hi = Φi/i−1 M − AMi giving the estimated input ˆ i/i−1 measurement Zˆ i = Hi Φi/i−1 M which is subtracted from actual measurement Zi to obtain the measured ˆ i−1 . residual or error ei0 = Zi − Hi Φi/i−1 M The error ei0 is premultiplied by the matrix Ki ˆ i/i−1 to give and the result is added to M the current estimate Mi∗ , see Eq. (110).

Modification The component R0 i∗ of the estimate Mi∗ = (Ri∗T , R0 i∗T )T is modified using a geometric constrain according the Eq. (121). T ˆ i = (Rˆ iT , Rˆ 0 i )T Then the final estimate M is stored until the next measurement is made, at which the cycle is repeated.

Figure 8.

Representation of the MEKF algorithm.

221

The MEKF would run recursively completing these cycles until instant time N . It is necessary to mention that Kalman filter implementation is sensitive to the numerical instability. In order to overcome these problems several techniques are available as the square-root filtering and the so called U-D factorization [19]. 7.

Analytical Method for the Recovering of Motion Parameters

In the experimental part we will compare the performance of the MEKF algorithm with an analytical method used by Zhang and Faugeras [22]. In this section, we explain briefly this method. This procedure can also be advantageously used for estimating the initial prediction in our MEKF algorithm. Consider k = 1, 2, . . . , n lines belonging to a rigid object. The lines Lk0 = nk0 + I mk0 are measured before the rigid motion transformation given by ¶ µ ti Ri (122) Mi = 1 + I 2

222

Bayro-Corrochano and Zhan

and the lines Lik = nik + I mik are measured after. According to the Eq. (34) the relation between a line L0 = Lk0 and the transformed line Li = Lik is given by Li = ni + I mi = Ri n0 R˜ i + I (Ri m0 R˜ i + ti ∧ (Ri n0 R˜ i )).

(123)

The real and dual parts of this equation are ni = Ri n0 R˜ i mi = Ri m0 R˜ i + ti ∧ ni .

(124) (125)

The method uses the noisy measurements Lk0 and Lik in the least square sense to estimate the best solution for the rotation and translation components. The first criterion of minimization uses the rotation of Eq. (124) ) ( n ° X ° k k ˜ °2 ° n − Ri n Ri . (126) E min = i

0

k=1

Min

where Ri = (Ri ) Rl (R˜ i ) Rr ,

(133)

and the matrix (ni )× is the skew-symmetric matrix of ni   0 n 3,i −n 2,i   0 n 1,i , (ni )× = −n 3,i (134) n 2,i −n 1,i 0 which performs the outer product of the bivector ni with another bivector. The estimate of the translation ˆti is obtained minimizing the following criterion ( 00 E min

=

n ° X ¡ ¢ ° °mk − R ˆ i mk0 + nik ˆti °2 i × k=1

) .

(135)

Min

After differentiating this criterion with respect to t0 and setting the result equal to zero, we obtain

Multiplying from the right both sides of (124) with the rotor Ri results

n X ¡ ¡ ¢ ¢ ¡ ¢ ˆ i mk0 + nik ˆti T nik = 0. 2 mik − R × ×

(136)

k=1

ni Ri − Ri n0 = 0.

(127) Finally the desired ˆti can be gained solving the following equation

This expression in terms of linear algebra reads (ni ) Rl Ri − (n0 ) Rr Ri = A R Ri = 0.

(128)

Thus Eq. (126) can be reformulated as follows ( ) n X © ª T 0 = RiT AkR AkR Ri = RiT A Ri Min , E min k=1

Min

A=

n X ¡ ¢ ¢ ¡¡ k ¢ ni Rl − nk0 Rr , k=1 n X

T

AkR AkR .

! n n X X ¡ k ¢T ¡ k ¢ ¡ k ¢T ¡ ¢ ˆ i mk0 − mik . ni × ni × ˆti = ni × R k=1

k=1

(137)

(130)

It is well known that at least two non-parallel lines are required to determine a unique motion displacement. tells that the matrices A and B = Pn This k k T (n ) (n k=1 i × i )× are always of full rank if two of the k lines Li (k = 1, . . . , n) are non-parallel.

(131)

8.

(129)

where AkR =

Ã

Experimental Results

k=1

Since A is a symmetric matrix and kRi k = 1, the solution to this problem is given by the smallest eigenvalue of A which corresponds to the 4-dimensional vector Rˆ i . Using the computed rotation Rˆ i the translation is straightforwardly computed with Eq. (125). In terms of linear algebra, Eq. (125) can be expressed as mi = Ri m0 − (ni )× ti ,

(132)

This section presents first a simulation to compute the motion parameters of an object moving in a screw trajectory. In this simulation the MEKF algorithm and the analytical solution are compared. The second experiment involves the use of the MEKF for the estimation of the motion parameters of a line moving in 3D space. The line measurements were taken using a stereo system attached to a 6 DOF robot arm.

Motor Extended Kalman Filter

223

were corrupted using noise variables n ωi and n vsi which are independent, normally distributed with zero mean and known deviations σω and σvs . We use the Eq. (99) of the ideal dynamic motion equation of a motor expressed in terms of linear algebra Mi = V Ml Mi−1 ,

(140)

in this sense the ground truth of the motor trajectories M0,i can be computed as follows M0,i = (r0i

r1i

r2i

r3i

r0i0

r1i0

r2i0

= Vi/i−1 Ml M0,i−1 , Figure 9.

8.1.

The object moving in 3-D with screw trajectory.

Comparison of the MEKF and Analytical Solution Algorithms

This simulation was written using MATLAB to compare both methods. The methods estimate the motion parameters of a moving object in the 3D space. The Fig. 9 shows a rigid object moving with screw motion about the known line axis Ls = r¯ + I tc ∧ r¯ = 0.7071γ2 γ3 + 0.3536γ3 γ1 + 0.6124γ1 γ2 + I (−0.7418γ2 γ3 + 0.3813γ3 γ1 + 0.6364γ1 γ2 ). with angular velocity w/2 = −φ/15 and a constant velocity vs = 0.3¯r. Since the measurements are sampled at equidistant intervals, they are normalized to 1. The motion between the instant times i − 1 to i was represented using the Eq. (100) of the motor V = (1 + I vs0 /2)(cos(ω/2) + sin(ω/2)Ls ) = (1 + I 0.3¯r/2)(cos(−π/15) + sin(−π/15)Ls ) = 0.9832 − 0.1289γ2 γ3 − 0.0645γ3 γ1 − 0.1117γ1 γ2 + I (−0.0266 + 0.2367γ2 γ3 −0.0188γ3 γ1 − 0.0283γ1 γ2 ).

(138)

r3i0 )T (141)

using as initial state M0,0 = (1 0 0 0 0 0 0 0). At the i-th instant of time in the MEKF algorithm two 3D points x00 and y00 define the line L00 at the coordinate frame A. After the application of the motor Mi = (Vi/i−1 ) Ml Mi−1 this line relative to the frame A is Li0 . Note that the real part of Mi gives the rotation Ri and from the dual part Ri0 using the Eq. (20) we can gain the expected translation ti = 2Ri0 R˜ i . Now having two lines related to the frame A: at the initial position L00 and at the position at time i the line Li0 , we can simulate the noisy line observations simply adding to those lines independent Gaussian noise with zero mean and known standard deviation σ, obtaining the noisy line observations L0 and Li . Using these noisy line observations both methods estimated the motion parameters with different degree of accuracy. The MEKF algorithm was tuned according to Eq. (121) in order to improve its accuracy and the convergence rate of the estimate. The Fig. 10 shows the eight components of the motor parameters for a motion trajectory of 20 time instants. The initial prediction of the MEKF was estimated using the analytic solution. This type of initial prediction (using the analytical method) works very well, because in all eight parameters the estimations align themselves quickly to the ground truth. We can clearly see that the MEKF algorithm follows the ground truth remarkably better than the analytical method. 8.2.

Estimation of the Relative Positioning of an Robot End-Effector

In the simulation the variables of the used motor Vi/i−1 = (1 + I vsi /2)(cos(ωi /2) + sin(ωi /2)Ls ) ωi = ω + n ωi , vsi = vs + n vsi ,

(139)

In this experiment we applied the MEKF algorithm to estimate the relative motion between the end-joint of a Staubli RX90 robot arm and 3D reference line belonging to a rigid object. The 3D line parameters were recovered during the arm movement using a stereo

224

Bayro-Corrochano and Zhan

Figure 10.

The estimation of the motor parameters for the simulated object motion.

vision system. This approach can be used for several industrial applications like maneuvering and grasping. The physical setup of the experiment is shown in Fig. 11. The system looks at a couple of lines lying on

Figure 11.

The physical setup of the experiment.

the floor and moves in the 3D space always conserving those lines in field of view. The main task for the system is to estimate automatically the relative motion between the floor lines and its end-joint. The visual system consists of two grey-scale CCD 640×480 cameras fastened to the last joint of the robot arm. The Staubli robot arm has six joints which can be controlled by six variables (x, y, z, roll, pitch and yaw). The coordinates (x, y, z) describe the position of the tool coordinate system T of the end-joint, referred to the coordinate system W of the robot. The orientation of the end-joint is described by the variables (roll, pitch, yaw) in terms of Euler angles. Since in practice we need three views in order to reconstruct a 3D line, using a two cameras stereo vision system we can create a virtual third one simply moving a bit the stereo system as it is illustrated in Fig. 12. The movement of the robot arm is carried out and controlled by the relative position and orientation between the tool coordinate system T and the base system W . The camera calibration procedure obtains

Motor Extended Kalman Filter

Table 1.

Reconstructed 3-D lines.

Time Line 0 1 2 .. . 14 15

225

A point on the line

Direction

1

(0.000

3.087 –2.327) (–0.345 0.937 –0.027)

2

(0.556

0.000 –2.250)

(0.941 0.336

0.023)

1

(1.125

0.000 –2.027) (–0.404 0.914

0.013)

2

(0.701

0.000 –2.049)

(0.915 0.401

0.029)

1

(1.111

0.000 –1.82)

(–0.462 0.886

0.017)

2

(0.794

0.000 –1.83)

(0.880 0.471

0.055)

1

(0.018

0.000

0.648) (–0.971 0.236 –0.036)

2

(1.103

0.000

0.538)

1

(–0.680

0.000

0.753) (–0.986 0.159 –0.025)

2

(0.000 –6.341

0.783)

(0.241 0.965

0.103)

(0.171 0.985 –0.003)

Figure 12. The relationship between the tool system T and the camera system C.

The motor Mi+1 expressed in terms of linear algebra is given by the projective transformation matrix P which relates up to scalar the visual 3D space to the image plane. The coordinate system of the camera C at the end-joint is related to the tool system frame T via a certain transformation X which was computed using a hand-eye calibration procedure [18]. When the tool system T is transformed from T1 to T2 by the transformation T, the camera system C will be transformed from C1 to C2 by a certain transformation C C = XTX−1 .

(145)

initialized with M0 = (1 0 0 0 0 0 0 0)T . The reconstructed 3D lines listed in Table 1 were used to estimate the relative motion between the end-join and the object on the floor using the MEKF algorithm. The procedure followed in this experiment is summarized below.

(142)

Since the motion of the robot arm specified by the transformation T is known, we can compare it with the relative motion C. The relative motion of the frame W and the frame C is a screw motion with constant angular velocity w = −π/90 and a constant translation velocity vs = 0.2. The axis line Ls is parallel to the z axis of the system C and the point (1.5, 0, 0) touches these axis line. In the motor algebra G+ 3,0,1 the axis line is given by Ls = γ1 γ2 + I (1.5γ2 γ3 ) ∧ (γ1 γ2 ) = γ1 γ2 + I 1.5γ3 γ1 .

Mi = V Ml Mi−1 ,

Step

Procedure

1

Cameras calibration to obtain P for each camera.

2

Hand-eye calibration to obtain X for each camera.

3

Robot arm movement images taken at constant sample rate, see Figs. 13 and 14.

4

Extraction of 2D lines from the images using the Hough transform, see Figs. 13 and 14.

5

3D line reconstruction using matched lines of three images, see Table 1.

6

Estimation of the motion using the 3D lines observations and the MEKF algorithm, see Fig. 15

(143)

Similarly as in the Eq. (138) the motor V is calculated as follows V = (1 + I vs0 /2)(cos(ω/2) + sin(ω/2)Ls ) = 0.9994 − 0.0349γ1 γ2 + I (0.0035 − 0.0523γ3 γ1 + 0.0999γ1 γ2 ). (144)

The algorithm for the motion estimation runs online recursively following the steps 3 to 6. The Fig. 15 presents the eight estimated parameters of the motion for 15 instant times. We can see clearly that after almost 4 observations the MEKF algorithm starts to follow almost perfectly the ground truth of the eight parameters. This real experiment as well as the previous simulation confirm that the MEKF algo-

226

Bayro-Corrochano and Zhan

Figure 13.

A stereo triplet of a sample object at time i = 0 with its edge images overlapped by extracted 2-D lines.

Figure 14.

A stereo triplet of a sample object at time i = 4 with its edge images overlapped by extracted 2-D lines.

Figure 15. The estimated motor parameters by the MEKF for the visual guided robot system.

rithm is an appropriate tool for the estimation of screw transformations using line observations. 9.

Conclusion

In this paper we modeled the motion of lines in the 4D space using the motor algebra. This kind of modeling linearize the 3D Euclidean rigid motion transformation.

The model of motion of lines using motors is very appealing for the design of an extended Kalman filter for the motion estimation. For the design of the filter we started with the rotor extended Kalman filter. As a natural extension of it we set the theoretic foundations of the motor extended Kalman filter (MEKF). The MEKF algorithm has the virtue of estimating the rotation and the translation transformations simultaneously. Since the most of recursive algorithms in the literature compute the translation and rotation transformations separately we can claim that this is one of the most important advantages of the MEKF. Additionally using the modeling of the lines in the motor algebra we could linearize the nonlinear measurement model which does not face singularities. The dynamic motion model using motors as states is useful to formulate effectively and to compute the screw motion of a line. In the algorithm of MEKF, we modified the step of the estimation to satisfy certain geometric constraints, which made the estimation converge faster to a proper motor state. Tests with simulated data confirmed that the tuning of the MEKF parameters improve substantially the MEKF capabilities. We compared the MEKF with an analytical method using simulated data. The comparison showed that the MEKF is a better estimation approach for the dynamic motion problem and due to its nature it can be used as online approach as opposed to batch methods. We presented also a real application of visual guided robot manipulation. This experiment aims to show the MEKF as an online algorithm for real applications. The vision system was calibrated beforehand using controlled robot movements and an effective hand-eye calibration method. Thereafter the system is ready to supply

Motor Extended Kalman Filter

the 3D line coordinates to the MEKF. The recovery of the parameters of the 3D lines was carried out using a stereo vision system, techniques of Hough transform and shape filtering for the line matching. Using the 3D lines the MEKF algorithm estimated efficiently the relative motion between its end-joint and a 3D reference line. Since the primer aim of this paper is to show the MEKF algorithm working online, we are sure there will be more accurate and faster methods for camera calibration and hand-eye calibration for future applications, therefore the use of the MEKF will be even more attractive for the estimation of screw motions using 3D line observations. Our future research includes the use of MEKF for robot localization, obstacle avoidance for mobile robots and control of binocular or trinocular heads.

References 1. K.S. Arun, T.S. Huang, and S.D. Blostein, “Least-Squares fitting of two 3D point sets,” IEEE Trans. Pattern Anal. Machine Intell., Vol. 9, No. 5, pp. 698–700, 1987. 2. A.J. Azarbayejani, H. Bradley, and A. Pentland, “Recursive estimation of structure and motion using relative orientation constraints,” In IEEE Conference on Computer Vision and Pattern Recognition, Los Alamitos, CA, June 1993, pp. 294–299. 3. I.Y. Bar-Itzhack and Y. Oshman, “Attitude determination from vector observations: Quaternion estimation,” IEEE Trans. on Aerospace and Electronic Systems, Vol. 21, No. 1, pp. 128–135, 1985. 4. E. Bayro-Corrochano, “The geometry and algebra of kinematics,” In Geometric computing with clifford algebra, G. Sommer (Ed.), Springer-Verlag, 2000. 5. E. Bayro-Corrochano, S. Buchholz, and G. Sommer, “Selforganizing clifford neural network,” in Proceedings of the International Conference of Neural Networks, ICNN’96, June 3–6, 1996, Washington D.C., USA, Vol. 1, pp. 120–125. 6. E. Bayro-Corrochano, K. Daniilidis, and G. Sommer, “Motor algebra for 3D kinematics: The case of the hand-eye calibration,” Journal of Mathematical Imaging and Vision, October 2000, Vol. 13(??), pp. 79–99. 7. J. Laseby and E. Bayro-Corrochano, “Analysis and computation of projective invariants from multiple views in the geometric algebra framework,” IJPRAI, Vol. 13, No. 8, 1999, pp. 1105– 1121. 8. W.K. Clifford, “Preliminary sketch of bi-quaternions,” in Proc. London Math. Soc., Vol. 4, pp. 381–395, 1873. 9. W.K. Clifford, “Applications of Grassmann’s extensive algebra,” Am. J. Math. Vol. 1, pp. 350–358, 1878. 10. K. Daniilidis and E. Bayro-Corrochano, “The dual quaternion approach to hand-eye calibration,” IEEE Proceedings of ICPR’96 Viena, Austria, Vol. I, Aug. 1996, pp. 318–322. 11. H. Grassmann, “Der Ort der Hamilton’schen Quaternionen in der Ausdehnungslehre,” Math. Ann., Vol. 12, p. 375, 1877. 12. D. Hestenes, “Space-time algebra,” Gordon and Breach, 1966.

227

13. D. Hestenes, New Foundations for Classical Mechanics, D. Reidel: Dordrecht, 1986. 14. D. Hestenes and G. Sobczyk, Clifford Algebra to Geometric Calculus: A Unified Language for Mathematics and Physics, D. Reidel: Dordrecht, 1984. 15. R. Horaud and F. Dornaika, “Hand-eye calibration,” Intern. Journal of Robotics Research, Vol. 14, pp. 195–210, 1995. 16. E.J. Lefferts, F.L. Markley, and M.D. Shuster, “Kalman filtering for spacecraft attitude estimation,” AIAA Journal on Guidance, Control, and Dynamics, Vol. 5, pp. 417–429, 1982. 17. R.E. Kalman, “A new approach to linear filtering and prediction problems,” Trans ASME Journal of Basic Engineering, Vol. 82, pp. 35–45, 1960. 18. S. Kunze, “Ein Hand-Auge-System zur visuell basierten Lokalisierung und Identifikation von Objekten,” Ms. Thesis, Christian-Albrechts-Universit¨at zu Kiel, Institut f¨ur Informatik und Praktische Mathematik, 1999. 19. P. Maybeck, Stochastic Models, Estimation and Control, Vol. 1, Academic Press: New York, 1979. 20. R. Sabata and J.K. Aggarwal, “Estimation of motion from a pair of range images: A review,” CVGIP: Image Understanding, Vol. 54, pp. 309–324, 1991. 21. H.W. Sorenson, “Kalman filtering techniques,” in Advances in Control Systems Theory and Applications, C.T. Leondes (Ed.), Vol. 3, Academic Press: New York, pp. 218–292, 1966. 22. Z. Zhang and O. Faugeras, 3D Dynamic Scene Analysis. Springer-Verlag: Berlin 1992.

Eduardo Bayro-Corrochano gained his Ph.D. in cognitive computer science in 1993 from the University of Wales at Cardiff. From 1995 to 1999 he was researcher and lecturer at the Institute for Computer Science, Christian Albrechts University, Kiel, Germany, working on the applications of Clifford geometric algebra to cognitive systems. He is a contributor, an editor and an author of books on applications of Clifford geometric algebra in computer science and engineering. He is also reviewer in various journals. In September 1997 he acted as program chair in the Int. Workshop Algebraic Frames for the Perception-Action Cycle, AFPAC’97, held in Kiel, Germany. In July 1999 together with Garret Sobczyk, he organized and chaired the Int. Workshop on Applications of Geometric Algebra in Computer Vision, Robotics, Computer Science and Engineering AGACSE’99 during the 5th International Conference on Clifford Algebras and their Applications in Mathematical Physics, held in Ixtapa-Zihuatanejo, Mexico. His current research interests include geometric neural networks, intelligent control, computer vision, visually guided robotics, geometry in color image processing and the use of bivector Lie algebras for perception of invariants and robot maneuvering. In his research and academic activities he promotes indefatigably a geometric approach for the design and implementation of real time artificial perceptionaction systems.

228

Bayro-Corrochano and Zhan

China. From 1995 to 1997 he worked in China as a consultant engineer in computer networking. Since 1998 he has been pursuing his Ph.D. at the Institute for Computer Science, Christian Albrechts University, Kiel, Germany, working on the applications of Clifford geometric algebra for visual guided robotics. His research interests involve Kalman filter techniques, multivector estimation, intelligent control, image processing and robotics.

Yiwen Zhang gained his M.S. degree in Electronic and Electrical Engineering in 1994 from the Fudan University of Shanghai, P.R.