Optimal Estimation of Dynamic Systems, 2nd Edition

436 Optimal Estimation of Dynamic Systems

(,,)

33 3

XYZ

22 2

(,,)XYZ

X

Y

Z

z

y

x

(,,,)XYZA

cc c

11 1

(,,)XYZ

Beacon 1

Beacon 2

Beacon 3

Object

Space

Image

Space

PSD

Wide Angle

Lens

Figure 6.16: Vision Navigation System

based on Position Sensing Diode (PSD) technology,

50

depicted in Figure

6.16. If we choose the z-axis of the sensor coordinate system to be directed

outward along the boresight of the PSD, then given object space (X,Y,Z) and

image space (x,y, z) coordinate frames (see Figure 6.16), the ideal object to

image space projective transformation (noiseless) can be written as follows:

x

i

= −f

A

11

(X

i

−X

c

)+A

12

(Y

i

−Y

c

)+A

13

(Z

i

−Z

c

)

A

31

(X

i

−X

c

)+A

32

(Y

i

−Y

c

)+A

33

(Z

i

−Z

c

)

, i = 1, 2,..., ...