Chih W i YiChih‐Wei Yi

Dept. of Computer Science

National Chiao Tung University

Contents Coordinate Systems

S  f   th f  b d  f   Sensor frame, earth frame, body frame, …

Changing Coordinate 

f Linear transforms

Coordinate transform matrics

Rotation in 3D space

Yaw, Pitch, Roll

Learn Coordinate Transform Matrices 

What Are their Readings?g

)0,2

2,

2

2( )0,1,0(

22

Sensor Frame "Sensor Frame" is the coordinate system used by h    d i  f  the sensor devices for sensor readings. 

The coordinate depends on The coordinate depends on the assembling of sensor ICs and device calibration. The direction of x‐axis, y‐The direction of x axis, yaxis, and z‐axis are indicated by hardware specs and follow the right specs and follow the right hand rule.

"s‐frame" is shorthand for "sensor frame"sensor frame .

Earth Frame The coordinates is used in tracking objects moving on the groundthe ground.

x‐axis points to the north pole.

i   i t  t  th   t y‐axis points to the east.

z‐axis points to the ground.

" f " i   h h d f   hi   i "e‐frame" is shorthand for this convention.

Body Framey The coordinate is corresponding to the pedestrian and called body framescalled body frames.

x‐axis points to the walking direction of the pedestrian.

i   i t  t  th   i ht  f th   d t i y‐axis points to the right of the pedestrian.

z‐axis points to the ground.

“b f " i   h h d f   hi   di   i “b‐frame" is shorthand for this coordinate convention.

Aircraft Onboard Coordinate The coordinate is used in areospace navigations.

i   i t  t  th  f t  f th   i ft x‐axis points to the front of the aircraft.

y‐axis points to the left of the aircraft.

i   i     h     f  h   i f z‐axis points to the top of the aircraft.

"FLT" is shorthand for this convention.

Vehicle Onboard Coordinate The coordinate is corresponding to the p gvehicle and called vehicle frames.

i   i t  t  th  f t  x‐axis points to the front of the vehicle.

y‐axis points to the right y p gof the vehicle.

z‐axis points to the bottom of the vehiclebottom of the vehicle.

"v‐frame" is shorthand for this coordinate convention.

Basis The vectors v1,v2, ,vn are a basis for a vector space V if and only ifand only if v1,v2,⋯,vn are linearly independent.

v1,v2,⋯,vn span V.1, 2, , n p

Ex: {e1,e2,e3} is a standard basis of R3. In addition, 

231201

111

102 and

111

011

both are bases of R³. Any basis of R³ must have exactly three elements.

Dimension Let V be a vector space. If V has a basis consisting of nvectors  we say that V has dimension nvectors, we say that V has dimension n. The subspace {0} of V is said to have dimension 0.

V i   id t  b  fi it di i l if th  i    fi it   t  f  V is said to be finite‐dimensional if there is a finite set of vectors that spans V. Otherwise, we say that V is infinite‐dimensional.infinite dimensional.

Rn and Rn×m are finite‐dimensional vector spaces.

Cn-1[a,b] is an infinite‐dimensional vector space.p

Coordinates Assume B={v1,v2, ,vn} is a (ordered) basis of a vector space Vspace V.

Any vector u in V can be expressed as a unique linear combination of the basiscombination of the basis.

Assume u=c1v1+c2v2+⋯+cnvn and the order of v1,v2,⋯,vnis fixed. is fixed. 

Then, (c1,c2,⋯,cn)BT is shorthand for the linear 

combination c1v1+c2v2+⋯+cnvn, and called as the 1 1 2 2 n n,coordinate vector of u with respected to the ordered basis B.

Examples of Coordinatesp Let B ={(1,0,1)T,(1,-1,0)T,(0,0,1)T} and 

B ={(1 2 1)T (0 -1 0)T (0 0 1)T} and consider the vector B {(1,2,1) ,(0, 1,0) ,(0,0,1) } and consider the vector (1,1,1)T (w.r.t. the standard basis). Since (1,1,1)T=2(1,0,1)T+(-1)(1,-1,0)T+(-1)(0,0,1)T, it has ( ) ( ) ( )( ) ( )( )coordinate (2,-1,-1)B₁

T.

Since (1,1,1)T=(1,2,1)T+(0,-1,0)T+0(0,0,1)T, it has di t  (1 1 0) Tcoordinate (1,1,0)B₂

T.

We can see that non‐zero vectors may have different coordinate vectors w.r.t. different basis.coordinate vectors w.r.t. different basis.

Now, we know that (2,-1,-1)B₁T= (1,1,0)B₂

T. Can we convert coordinates w.r.t. two bases?

An Example of ChangingAn Example of Changing Coordinates  Step 1,

(1 0 1) (1 2 1)+2(0 1 0)+0(0 0 1) (1,0,1) = (1,2,1)+2(0,-1,0)+0(0,0,1)

(1,-1,0) = (1,2,1)+3(0,-1,0)+(-1)(0,0,1)

(0 0 1) 0(1 2 1) 0(0 1 0) (0 0 1) (0,0,1) = 0(1,2,1)+0(0,-1,0)+(0,0,1)

Step 2,

(1,1,1) = 2(1,0,1) + (-1)(1,-1,0) + (-1)(0,0,1)= 2((1,2,1) + 2(0,-1,0) + 0(0,0,1))

+ ( 1)((1 2 1) + 3(0 1 0) + ( 1)(0 0 1))+ (-1)((1,2,1) + 3(0,-1,0) + (-1)(0,0,1))+ (-1)(0(1,2,1) + 0(0,-1,0) + (0,0,1))

= (1,2,1)+(0,-1,0)+0(0,0,1)( , , ) (0, ,0) 0(0,0, )

A Simple Observationp

20111 .1

2

032

011

1

1

111001212 BBBB

From the Other Side Step 1,

(1 2 1) 3(1 0 1)+( 2)(1 1 0)+( 2)(0 0 1) (1,2,1) = 3(1,0,1)+(-2)(1,-1,0)+(-2)(0,0,1)

(0,-1,0) = (-1)(1,0,1)+(1,-1,0)+(0,0,1)

(0 0 1) 0(1 0 1) 0(1 1 0) (0 0 1) (0,0,1) = 0(1,0,1)+0(1,-1,0)+(0,0,1)

Step 2,

(1,1,1) = (1,2,1) + (0,-1,0) + 0(0,0,1)= (3(1,0,1) + (-2)(1,-1,0) + (-2)(0,0,1))

+ (( 1)(1 0 1) + (1 1 0) + (0 0 1))+ ((-1)(1,0,1) + (1,-1,0) + (0,0,1))+ 0(0(1,0,1) + 0(1,-1,0) + (0,0,1))

= 2(1,0,1) + (-1)(1,-1,0) + (-1)(0,0,1).( ,0, ) ( )( , ,0) ( )(0,0, ).

Verify the Observationy

10132 .1

1

012

013

1

2

011212121 BBBB

Changing Coordinatesg g B ={v1,v2, ,vn} and B ={u1,u2, ,un} are two bases of a vector space V   If the coordinate vectors of w wr t  Bvector space V.  If the coordinate vectors of w w.r.t. Bis c=(c1,c2, ,cn)B₁

T, and the coordinate vectors of wwr t  B is d=(d1 d2 d )B

T  what is the relation w.r.t. B is d (d1,d2, ,dn)B₂ , what is the relation between c and d? Assume vi=a1iu1+a2iu2+ +a iu for any 1≤i≤n   ThenAssume vi a1iu1+a2iu2+…+aniun for any 1≤i≤n.  Then,

nnnnnccc vvvw

2211

nnn

ii

inii

iii

i

cacaca

acacac

uuu

uuu

1

11

211

11

nj

jnjj

jjj

jj cacaca uuu

1

21

211

1

Changing Coordinates (Cont.)g g ( ) So, we have di=ai1c1+ai2c2+…++aincn.  In other words, 

caaad

2

1

22221

11211

2

1

n

n

c

c

aaa

aaa

d

d

Similarly, if ui=b1iv1+b2iv2+…+bnivn for any 1≤i≤n. Then, 1212

21 BnBBnnnnBn caaad

2

1

22221

11211

2

1

n

n

d

d

bbb

bbb

c

c

Furthermore, AB=I.2121

21 BnBBnnnnBn dbbbc

,

Linear Transformations V and W are vector spaces. A function L:V→W is called a linear transformation if L(au+bv)=aL(u)+bL(v)called a linear transformation if L(au+bv)=aL(u)+bL(v)for any scalars a,b and vectors u,v V.

Example:  Example: 

x

yxL2

Problem:

Is L([x])=[x+1] a linear transformation?

x2

Is L([x])=[x+1] a linear transformation?

Let A is a 2×3 matrix and v∈R³. Prove that L(v)=Av is a linear transformation form R³ to R²linear transformation form R to R .

2D Image Processingg g Many image processing techniques can be implemented by linear transformations  e g  implemented by linear transformations, e.g., 

Dilations and contractions

R fl ti Reflections

Rotations

T l ti ??? Translations???

Composition of LinearComposition of Linear Transformations U, V, and W are vector spaces.  B , B , and B are bases of U  V  and W  respectively   L :U→V and bases of U, V, and W, respectively.  L :U→V and L :V→W are linear transformations.  Let A be the matrix representation of L wr t  B and B  and B be matrix representation of L w.r.t. B and B , and B be the matrix representation of L w.r.t B and B . Then, 

L₂∘L₁ is a linear transformationL₂ L₁ is a linear transformation.

The matrix representation of L₂∘L₁ w.r.t. B₁ and B₃ is BA.

Similarity of Matricesy A and B are n×nmatrices. We say B is similar to A if there exists a nonsingular matrix S such that B=S⁻¹AS. g

Example:  Let  p

.11

11 and ,

10

12 ,

11

02

UBA

We have 

.21

21

1

U

andB=U⁻¹AU.    

21

21

U U.

Linear Transformations andLinear Transformations and Transition Matrices V is a n dimension vector space, and L:V→V is a linear transformation  B and B are bases of V  and S is a transformation. B and B are bases of V, and S is a transition matrix from B to B .  Let A be the matrix representation of L wr t  B  and B be the matrix representation of L w.r.t. B , and B be the matrix representation of L w.r.t. B . Then, we have B=S ¹AS.

What are Euler Angles?g Eular Angles are used in areospace engineering to describe the posture of aircraft  describe the posture of aircraft. 

Recall that the FLT convention is defined as followsRecall that the FLT convention is defined as follows. x‐axis points to the front of the aircraft. 

y‐axis points to the left side of the aircraft.y axis points to the left side of the aircraft.

z‐axis points to the top of the aircraft.

Eular angles consisits of three components include yaw ψ, pitch θ, and roll φ.

Yaw, Pitch, and Roll, ,

Yaw The aircraft rotates by the z axis in the clockwise z‐axis in the clockwise manner, i.e. in the direction from the y‐axis to the x‐axis  from the y axis to the x axis. 

The angle is denoted as ψwith range (-π π]with range ( π,π].

The transition matrix from the new coordinate system 

0cossin

0sincos

Ythe new coordinate system back to the old one is Yψ.

100

Pitch The aircraft rotates by the y axis in the clockwise y‐axis in the clockwise manner, i.e. from the x‐axis to the z‐axis  to the z axis. 

The angle is denoted by θwith range [-(π/2) (π/2)]with range [ (π/2),(π/2)].

The transition matrix from the new coordinate system 

010

sin0cos

Pthe new coordinate system back to the old one is Pθ.

cos0sin

010P

Roll The aircraft rotates by the x axis in the x‐axis in the counterclockwise manner, i.e. from the y‐axis to the z‐axisfrom the y axis to the z axis.

The angle is denoted by φwith range [-π π]with range [ π,π].

The transition matrix from the new coordinate system 

sincos0

001

Rthe new coordinate system back to the old one is Rφ.

cossin1

Yaw‐Pitch‐Raw Convention  A coordinate system rotation is composed of yaw, pitch, and roll  However, the three operations are not pitch, and roll. However, the three operations are not commutable. YPR, YRP, RYP, RPY, RYP, and RPY all are different.

Conventionally, the rotation is described in the order y,Y‐P‐R.

Note that the transition matrix will be given by YψPθRφinstead of RφPθYψ.

YPR Transition Matrices

RPY

sincos0

001

010

sin0cos

0cossin

0sincos

cossin0

sincos0

cos0sin

010

100

0cossin

ψθφφψψφθψφψθ

ψθφψφφψθψφ

ssc+sc-sss+ccsc-

csc-ss-scs-sccc

φθφθθ ccscs

Examplesp If a transition is sequentially composed of ψ=30°, θ=45°  and φ=60°   Let B be the basis of the original θ=45 , and φ=60 .  Let B be the basis of the original system and B be the basis of the new coordinate systemsystem.

What is the transition matrix TB₂→B₁?

What is the coordinate of the vector [10 0 10] T in B₁? What is the coordinate of the vector [10 0 10]B₂ in B₁?

A  t ti    [1 0 1] T t  [0 √2 0] T d  A rotation maps [1 0 1]B₁T to [0 √2 0]B₂

T and [1 0 0] B₁

T to [0 (√2)/2 (√2)/2] B₂T.

Fi d th  t iti   t i Find the transition matrix.

Find the corresponding Euler angles.

Find Transition MatricesFind Transition MatricesNotations TS→V: convert the sensor reading to the v‐frame.

T     h     di     h   f TS→E: convert the sensor reading to the e‐frame.

TV→E: convert the vectors in the f‐frame coordinate to h   f  the e‐frame. 

We haveTS→E = TV→E TS→V ;TV→E = TS→E (TS→V) ¹.

From S‐Frame to V‐Frame Assume the sensor is fixed on the vehicle. The transition matrix to transfer readings of the sensor to the vehicle gframe is denoted as TS→V. 

If sV and sS denote the sensor reading in the vehicle frame and sensor frame respectively, we have 

sV = T sSs TS→V s .

To find the transition matrix, we need at least three pairs of To find the transition matrix, we need at least three pairs of corresponding vectors between sV and sS. Let f₀,s₀,g₀ denote the coordinates in the s‐frame of the three 

i     f  h   i   i   d  i  i   h  f funit vector of the x‐axis, y‐axis, and z‐axis in the f‐frame.

Find the Vertical Vector in theFind the Vertical Vector in the V‐Frame Park the car on a horizontal plane, and then measure the g value  the g‐value. 

Th    i   di     h   i  b     The vector is corresponding to the gravity but note that the readings point upward. Denote the reading g. Then  g g/|g| should map e V  the unit vector of the Then, g =-g/|g| should map e V, the unit vector of the z‐axis in the v‐frame.

We use G to denote g/|g|.

Find the Forward Vector in theFind the Forward Vector in the V‐Frame On a horizontal plane, accelerate straightforward, and then measure the g‐value  Let g denote the reading  measure the g value. Let g denote the reading. 

Then  g G points forward Then, g-G₀ points forward.

f =((g G )/(|g G |)) is corresponding to e V  the unit vector  f₀=((g-G₀)/(|g-G₀|)) is corresponding to e₁V, the unit vector of the x‐axis in the v‐frame.

Find the Side Vector in the V‐Frame The cross product of g and f points to the right side of the vehicle  i e  s =g ×f   of the vehicle, i.e. s =g ×f .  

    h   V   h   i     f  h   i  i   h   s maps to the e V, the unit vector of the y‐axis in the vehicle frame.  

Put all together, I = TS→V [ f s g ]

and T [ f ] 1TS→V = [ f s g ]-1.

From S‐Frame to E‐Frame We make the following assumption.

A  th     d      li d Assume the g‐sensor and m‐sensor are aligned.

The magnetic field points to the north.

Find three pairs of correspondent vectors.

g₀ points to the direction of the z‐axis in the e‐frame.

The magnetic field points to the north that is the di i   f  h   i  i   h   f  L  b d   h  direction of the x‐axis in the e‐frame. Let b₀ denote the unified magnetic field.

The cross product of g and b points to the east The cross product of g₀ and b₀ points to the east.

From S‐Frame to E‐Frame (Cont.)( ) Then, 

I = T [ b g ×b g ]I = TS→E [ b g ×b g ]and

TS E = [ b g ×b g ]-1TS→E [ b g ×b g ]

Thinking ...g Can we find the transition matrix without the help of either g‐sensors or m‐sensors?either g sensors or m sensors?

The g‐sensor and m‐sensor are aligned∙ The g sensor and m sensor are aligned  

The magnetic field does not point to the north   It has  The magnetic field does not point to the north.  It has declination and inclination. How can we handle this?  

Can we directly find the transition matrix TV→Ewithout the help of s‐frame?p

Eular Angles Conversiong Assume there is a rotation (ψS,θS,φS) is measured in the s frame  Let (ψV θVφV) denote the rotation measured s‐frame. Let (ψV,θV,φV) denote the rotation measured in the v‐frame.

1 T new = T old Y S P S R S1. TS→Enew = TS→E

old YψS PθS Rφ

S,

2. TV→Enew = TV→E

old YψV PθV Rφ

V,

3 T old = T old T old3. TS→Eold = TV→E

old TS→Vold,

4. TS→Enew = TV→E

new TS→Vnew.

Note that TS→Vnew = TS→V

old with shorthand TS→V.

Eular Angles Conversion (Cont.)g ( ) Replace TS→E

new in Eq.(4) by Eq.(1) and TV→Enew in

Eq (4) by Eq (2)  Then  Eq.(4) by Eq.(2). Then, 

TS Eold Y S PθS R S = TV E

old Y V PθV R V  TS→E Yψ Pθ Rφ = TV→E Yψ Pθ Rφ . 

So   So, Yψ

V PθV RφV = (TV→E

old )-1 TS→Eold Yψ

S PθS RφS

= T old T old Y S P S R S= TE→V TS→E Yψ Pθ Rφ

= TS→V YψS PθS Rφ

S.

Introduction to QuaternionQ The quaternions, an extension of the complex numbers, form a noncommutative number system.y

A quaternion is denoted as either q₀+q₁i+q₂j+q₃k or q q q q j q(q₀,q₁,q₂,q₃). The set of quaternions, denoted as H, is a 4‐D space.

      d    l  b q₀, q₁, q₂, and q₃ are real numbers.

1, i, j, and k form a basis of the quaternion space.

H has three operations: addition, scalar multiplication, and quaternion multiplication.q p

Addition and Scalar MultiplicationAddition and Scalar Multiplication of QuaternionsQ The set of quaternions with the addition and scalar multiplication form a vector spacemultiplication form a vector space.

The addition of two quaternions q=q₀+q₁i+q₂j+q₃k and p=p₀+p₁i+p₂j+p₃k is given by p=p₀+p₁i+p₂j+p₃k is given by 

q+p = (q₀+p₀)+(q₁+p₁)i+(q₂+p₂)j+(q₃+p₃)k.q p (q p ) (q p ) (q p )j (q p )

The scalar mutiplication of a real number α and a quaternion q=q₀+q₁i+q₂j+q₃k is given by 

αq αq +αq i+αq j+αq kαq=αq₀+αq₁i+αq₂j+αq₃k.

Multiplication of Quaternionsp Q The multiplication of basis elements is given by 

i²=j²=k²=ijk= 1i²=j²=k²=ijk=-1. ij=k; ji=-k; jk=i; kj=-i; ki=j; ik=-j.

Th   lti li ti   f  t i i   t ti The multiplication of quaternions is noncommutative.

The multiplication of two quaternionsq + i+ j+ k and p + i+ j+ k is gi en bq=q +q i+q j+q k and p=p +p i+p j+p k is given by

qp = (q p -q p -q p -q p )+ (q p +q p q p +q p )i+ (q p +q p -q p +q p )i+ (q p +q p +q p -q p )j+ (q p -q p +q p +q p )k.+ (q p q p +q p +q p )k.

Quaternion Conjugate andQuaternion Conjugate and Augmented Pointg The conjugate of a quaternion q=q +q i+q j+q k, denoted by q∗  is given bydenoted by q , is given by

q∗ = q₀-q₁i-q₂j-q₃k.

( ∗) ( ∗) 2+ 2+ 2+ 2 q(q∗) = (q∗)q = q₀2+q₁2+q₂2+q₃2.

Th   d  i  f d f       The augmented point formed from a vector r=[ r r r ]T R³, denoted by  , is a quaternion given by

rby

krjrir 32100

r 321

r

Rotations Represented byRotations Represented by QuaternionsQ The rotation of an angle θ around a unit vector 

u=[ u u u ]T in counterclockwise can be u=[ u u u ]T in counterclockwise can be represented by a unit quaternion

sin

cos

cos 21

2

2

iu

.sin

s

sin

cos

22

21

2

2

ju

iu u

q

sin 23

ku

How to Apply Quaternions?pp y Q Let the vectors of R³ be represented by augmented points  points. 

Rotation of vector r R³ an angle θ around a unit vector Rotation of vector r R an angle θ around a unit vector u R³ can be computed as 

*' qrqr

The inverse rotation can be obtained by conjugating 

qq

j g gthe rotation quaternion 

qrqr '*

Transition Matrices Let T denote the transition matrix from r to r’, i.e. 

r’=Tr  From the properties of quaternions  we have the r =Tr. From the properties of quaternions, we have the transition matrix in the form

32102

32

22

12

02130

312021302

32

22

12

0

22

22

qqqqqqqqqqqq

qqqqqqqqqqqq

T

2

32

22

12

023101320 22 qqqqqqqqqqqq

Find the Change of Direction byFind the Change of Direction by Magnetometerg Let B and B be the magnetic field measured at two momentsmoments.

The rotation around a vector u=[u u u ]T of angle θtransforms B into Btransforms B into B .

One possible basis in which the rotation can be written 

1

B

Bxv

the rotation can be written as a simple rotation of angle θ around one axis k is given 

2

1212

2

1

1

BBBBB

BBBBB

B

yvθ around one axis k is given by the set of three vectors

21

21

12121

BB

BB

BBBBB

zv 21

Cont. Let b and b be the projection of B and B , respectively  in the basis given above  and respectively, in the basis given above, and 

θ=arccos((b b )/(|b | |b |)).Since the transition matrix is Since the transition matrix is 

222231202130

23

22

21

20

22

22

qqqqqqqqqqqq

qqqqqqqqqqqq

T

23

22

21

2023101320

321032102130

22

22

qqqqqqqqqqqq

qqqqqqqqqqqqT

we can obtain the yaw, pitch, and roll angles.

arctan;arcsin;arctan 3,22,1

T

TT

.arctan ;arcsin ;arctan3,3

3,11,1

T

TT