International Global Navigation Satellite Systems Society IGNSS Symposium 2009

Holiday Inn Surfers Paradise, Qld, Australia

1 – 3 December, 2009

Parameter Estimation for Mine Navigation

Garry Einicke CSIRO Exploration and Mining, Australia

Ph 07 3327 4615, Fax 07 3327 4566, garry.einicke@csiro.au

Gainluca Falco Politecnico Di Torino, Italy

Ph 011 5646666, Fax 011 5646329 , gianluca.falco@polito.it

John Malos CSIRO Exploration and Mining, Australia

Ph 07 3327 4114, Fax 07 3327 4566, john.malos@csiro.au

David Reid CSIRO Exploration and Mining, Australia

Ph 07 3327 4437, Fax 07 3327 4566, david.reid@csiro.au

David Hainsworth CSIRO Exploration and Mining, Australia

Ph 07 3327 4420, Fax 07 3327 4566, david.hainsworth@csiro.au

ABSTRACT

This paper discusses the convergence of a Kalman filter-based Expectation-Maximisation algorithm for estimating unknown model parameters. Under prescribed conditions, the parameter estimates converge to the actual values. Mining navigation applications are discussed in which good parameter estimates are required. An inertial alignment example is presented, which demonstrates that the sequence of process noise covariance estimates is monotonically decreasing. A position tracking application is also discussed in which it is demonstrated that the sequence of state matrix estimates is similarly monotonic decreasing. KEYWORDS: Kalman filtering, EM algorithm, parameter estimation, mine navigation.

1. INTRODUCTION Inertial navigation systems typically use optimal minimum-variance filters to track platform trajectories. However, attaining good tracking performance requires precise knowledge of the underlying state-space model parameters and noise statistics. An iterative technique for estimating these unknowns is the expectation-maximization (EM) algorithm, which is described in McLachlan and Krishnan (1997). This paper addresses the problem of estimating unknown transition matrices, process noise variances and measurement noise variances using the EM algorithm. The EM algorithm for parameter estimation consists of iterating two steps: an expectation step and a maximization step. The expectation step involves calculating optimal state estimates which relies on solving Riccati equations. The maximization step involves the calculation of decoupled maximum-likelihood estimates (MLEs). It is shown under prescribed conditions that the estimate sequences will be monotonic nonincreasing and asymptotically approach the exact values. The paper is organized as follows. The EM algorithms for estimation of the measurement noise variance, process noise variance and state matrix parameters are described in Section 2. When the initial estimates of the unknown parameters approach the actual values from above, the sequence of MLEs will be monotonically non-increasing. In the limit, when the measurement noise becomes negligible, the MLEs of the state matrix parameters and process noise variances asymptotically approach the actual values. Conversely, when the process noise is negligible, the MLEs of the measurement noise variances asymptotically approach the actual values. Two navigation applications, namely inertial system stationary alignment and position tracking, are described in Section 3. 2. EM ALGORITHMS 2.1 Measurement Noise Variance Estimation Consider a linear system having the state-space realization

1kx + = kAx + kw (1)

kz = kCx + kv , (2) where A ∈ n x n , C ∈ p x n , kx ∈ n , kw ∈ n , kv ∈ p . The sequences kw and kv are assumed to be independent, zero mean, stationary, white processes, with actual covariances E{ T

k kw w } = Q and E{ Tk kv v } = R , respectively.

We consider the case where the measurements (2) can be written as

1,

2,

,

...

k

k

p k

zz

z

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

=

1

2

...

p

cc

c

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

kx +

1,

2,

,

...

k

k

p k

vv

v

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

, in which cj , j = 1,…,p, refers to the jth row of C . Denote the

actual measurement noise covariance by R =

1

2

0 ... 00 ... 00 ... ... 00 ... 0 p

rr

r

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

where jr = { }Tj jE v v ∈ .

From the approach of Kay (1993), it is assumed that ,j kz ~ N( j kc x , )jr , i.e., the probability

density function of ,i jz is , ,/ 2 1

1 1( ) exp{ ((2 ) 2

N

j k j kN kj j

p z zr rπ =

= − ∑ 2) }− j kc x . By setting

,log ( )e j k

j

p zr

∂∂

= 0, it is straightforward to show that an unbiased MLE for the jth measurement

noise variance is given by

,1

1 (1

N

j j kkr z

N ==

− ∑ 2)− j kc x . (3)

Denote the estimated measurement noise covariance by iR =

,1

,2

,

0 ... 00 ... 00 ... ... 00 ... 0

i

i

i p

rr

r

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

, where ,i jr is

the ith estimate of jr . Suppose that a Kalman filter designed with iR has produced corrected state estimates, which are denoted by , /ˆi k kx . Let , , /ˆi j k kx denote the jth component of , /ˆi k kx . An EM algorithm for iteratively re-estimating iR arises by a finite repetition of the following two-step procedure.

Step 1) Use a Kalman filter designed with iR to calculate corrected state estimates , /ˆi k kx .

Step 2) For j = 1,…,p, use , , /ˆi j k kx within (3) to obtain 1iR + . It can similarly be shown that under prescribed conditions, if the initial estimates 0,ˆ jr approach

jr from above, the sequence ,i jr will be monotonically non-increasing. When the estimation problem is dominated by measurement noise, that is, when the ratio of the measurement noise to the process noise intensities is large, the measurement noise variance iterations converge to

the actual value, i.e., 10, 0,i

LimQ R i−→ → →∞ ir r= .

2.2 Process Noise Variance Estimation In respect of (1) - (2), assume that kw ∈ n consists of independent, zero-mean, white Gaussian, measurement noise sequences. Let , 1j kx + denote the jth row of 1kx + , then (1) may be written as

1, 1

2, 1

, 1

...

k

k

n k

xx

x

+

+

+

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

=

1

2

... k

n

aa

x

a

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

+

1,

2,

,

...

k

k

n k

ww

w

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

, where aj , j = 1,…, n, refers to the jth row of A. Denote the actual

process noise covariance by Q =

1

2

0 ... 00 ... 00 ... ... 00 ... 0 n

qq

q

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

, where jq = { }Tj jE w w ∈ . From the

approach of Kay (1993), it is assumed that , 1j kx + ~ N( j ka x , )jq , i.e., the probability density

function of , 1j kx + is 1

, 1 , 1/ 2 1

1 1( ) exp{ ((2 ) 2

N

j k j kN kj j

p x xq qπ

−

+ +== − ∑ 2) }j ka x− . By setting

, 1( )j k

j

p xq

+∂∂

= 0, it is straightforward to show that an unbiased MLE for the jth process noise

variance is given by 1

, 11

1 (2

N

j j kkq x

N−

+==

− ∑ 2)j ka x− . (4)

Denote the estimated process noise covariance by iQ =

,1

,2

,

0 ... 00 ... 00 ... ... 00 ... 0

i

i

i p

qq

q

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

, where ,i jq is the

ith estimate of jq . Suppose at iteration i that a Kalman filter designed with iQ has produced corrected state estimates, which are denoted by , /ˆi k kx . Let , , /ˆi j k kx denote the jth row of , /ˆi k kx . An EM algorithm for iteratively estimating Q arises by repeating the following two-step procedure.

Step 1) Use a Kalman filter designed with iQ to calculate corrected state estimates , /ˆi k kx .

Step 2) For j = 1,…,n, use , , /ˆi j k kx and , , 1/ 1ˆi j k kx + + within (4) to obtain 1iQ + . It is shown in Einicke et al (2009), that under prescribed conditions, if the initial estimates

0,ˆ jq approach jq from above, the sequence ,ˆi jq will be monotonically non-increasing. When the ratio of the process noise to the measurement noise intensities is large, the states are reconstructed exactly, in which case the process noise variance iterations converge to the

actual value, i.e., 10, 0,Lim

R Q i−→ → →∞ iq q= .

2.3 State Matrix Estimation In respect of the system (1), assume that A is a diagonal matrix, i.e., A = diag ( 1a , 2a , …, ja ,

… na ), 0 ≤ ja ≤ 1, j = 1, …, n. An estimate A = diag ( 1a , 2a , …, ˆ ja , … ˆna ) is sought which minimizes the residual error

1,i ks + = 1kx + − ˆkAx − kw (5)

in an optimum way. Let ,ˆi ja denote an estimate of ja at iteration i. Using a similar approach to that in Sections 2.1 and 2.2, an estimate 1,ˆi ja + can be calculated by

11 12

1, , , 1/ 1 , , / , , /0 0

ˆ ˆ ˆ ˆN N

i j i j k k i j k k i j k kk k

a x x x−− −

+ + += =

⎛ ⎞⎛ ⎞= ⎜ ⎟⎜ ⎟⎝ ⎠⎝ ⎠∑ ∑ (6)

where , , /ˆi j k kx denotes the jth component of , /ˆi k kx .

An EM algorithm for iteratively re-estimating ˆiA arises by a finite repetition of the following

two-step procedure. Step 1) Use the Kalman filter designed with ˆ

iA to calculate the filtered state estimates

, /ˆi k kx .

Step 2) For j = 1, …, p, use , , /ˆi j k kx within (6) to obtain 1ˆ

iA + .

It can be shown that under prescribed conditions, if the initial estimates 0,ˆ ja approach ja from above, the sequence ,ˆi ja will be monotonically non-increasing. When the measurement noise is negligible, the estimates converge to the exact values, i.e., ,0

ˆlim i j jRa a

→=

3. APPLICATIONS 3.1 Inertial Navigation System Alignment Our team is engaged in developing inertial navigation systems to automate longwall shearers (Figure 1) within underground coal mines. Inertial navigation systems are used to measure the coal face profile in three dimensional space. This information is used to keep the face straight, on track and in the seam. Inertial navigation systems possess three-axis accelerometer and gyro sensor assemblies. The inertial data is used to calculate estimates of the instantaneous orientation, velocity and position of the shearer. The modeling of orientation can be undertaken either by direction cosine matrices or by quaternions. Our development is based on the approach of Savage (2000) and employs direction cosine matrices and a tilt vector.

A rotation of a body in three dimensional space can be represented by a simple rotation matrix for each Euler angle, namely yaw, pitch and roll, which are shown in Figure 1. A direction cosine matrix is the product of these three rotation matrices. A standard calculation can be applied to transform the direction cosine matrix into a three dimensional tilt vector which is

Figure 1. Underground coal mine longwall shearer Euler angles.

also known as the orientation vector. Alignment is the process of estimating the Earth rotation rate and rotating the attitude direction cosine matrix, so that it transforms the body-frame sensor signals to a locally-level frame, wherein certain components of accelerations and velocities approach zero when the platform is stationary. This can be achieved via an alignment Kalman filter using the model

1/k kx + = / 1k kAx − + ku , (7) where, T

kx = [ ,X kδω , ,X kγ , ,X kvδ , ,X krδ ]T, ,X kδω ,X kγ , ,X kvδ and ,X krδ ∈ are the components of the error in earth rotation rate, tilt, velocity and position vectors respectively, and μk ∈ 4 is a deterministic signal which is a nonlinear function of the states, see Savage (2000). The

state transition matrix is given by A = I + sTΦ + 21 ( )2! sTΦ + 31 ( )

3! sTΦ , where Ts is the

sampling period and Φ =

0 0 0 01 0 0 00 0 00 0 1 0

g

⎡ ⎤⎢ ⎥⎢ ⎥⎢ ⎥⎢ ⎥⎣ ⎦

is the continuous-time transition matrix, in which g

is the universal gravitational constant. The output mapping is C = [0 0 0 1] . It is demonstrated below that the EM algorithm described in Section 2.2 can be used to estimate the unknown Q from measured data. Raw three-axis accelerometer and gyro data was recorded from a stationary Litton LN270 Inertial Navigation System at a 500 Hz data rate. From the conditions within Section 2.2, the initial parameter estimates and RDE solution need to be larger than the steady state values. That is, selecting arbitrarily large initial values will suffice. However, in order to generate a compact plot, the initial estimates were selected to be 10 times the steady state values. The diagonal components of Q, normalized by their value after 10 iterations, are shown in Figure 2. The red solid line, blue dashed line, green dot-dashed line and black dotted line indicate the normalised (1,1), (2,2), (3,3) and (4,4) components of Qi. The figure demonstrates that approximate MLEs approach the steady state values from above, which is consistent with the theory (Einicke et al, 2009).

0 5 100

5

10

15

20

i

Normalized Variances

Figure 2. Normalized components of Qi versus iteration number. The red solid line, blue dashed line, green dot-dashed line and black dotted line indicate the normalised (1,1), (2,2), (3,3) and (4,4) components of Qi.

The estimated Earth rotation rate magnitude versus time is shown in Figure 3. At 100 seconds, the estimated magnitude of the of the Earth rate is 72.53 micro-radians per second, that is, one revolution every 24.06 hours. This estimated Earth rate is about 0.5% in error compared with the mean sidereal day of 23.93 hours (Seidelmann, 1992). Since the estimated Earth rate is in reasonable agreement, it is suggested that the MLEs for the unknown Q are satisfactory.

0 20 40 60 803

4

5

6

7

8x 10

−5

t [sec]

|Ear

th r

ate|

[rad

/sec

]

3.2 Position Tracking The authors are involved in the development of automated systems that provide safety and productivity improvements within mines. In underground coal mines, longwall mine equipment is traditionally operated manually, which results in the face wandering out of the seam and coal product being contaminated by rock. We employ optimal filters and smoothers that operate on inertial sensor measurements to estimate face positions and automate longwall equipment. The developed longwall automation system keeps the face straight and removes workers from hazardous areas (Einicke et al, 2008). Historically, all surface mining equipment such as excavators, shovels and haul trucks have been operated manually. Mine personnel are being increasingly relocated to cities, where they control equipment remotely. Consequently, the authors are also involved in the development of automated systems to control remote surface mine equipment. This involves the use of optimal filters and inertial sensor measurements to calculate equipment position estimates. A wheeled robot running on a 236m-long test track has been established at the CSIRO to assess the performance of inertial sensors. The positions reported by a Northrop Grumman LN270 inertial navigation unit mounted on the platform recorded over 20 passes along the test

Figure 3. Estimated magnitude of Earth rotation rate.

track are indicated by the thin green lines within Figure 4. The true track positions were determined from an odometer and are indicated by the thick line within Figure 4. The figure shows that the inertial position errors increase with distance and large deviations occur when the robot’s wheels drive over obstacles on the track. It can be seen from the figure that worst case errors of 3 m occur in the vertical plane.

010

2030

−250−200

−150−100

−500

−3.5

−3

−2.5

−2

−1.5

−1

−0.5

0

East, mNorth, m

Alti

tude

, m

We seek to exploit map information, that is, use prior knowledge of the track position to calculate improved position estimates. Consider a third order system having a diagonal state matrix together with C = diag (1, 1, 1). Let ˆkx denote a 3 1× state vector for east, north and altitude error position states. Also, let INU

kz and MAPmz , m = 1, …, M, denote 3 1× vectors of

inertial navigation unit measurements at time k and known test track position coordinates, respectively. The measurements for a Kalman filter are given by NEAREST INU

k k kz z z= − where

21ˆarg minNEAREST MAP INU

k m k km M

z z z x≤ ≤

= − − are the nearest test track position coordinates.

The EM procedure described in Section 2.3 is used to jointly estimate the states and the state matrix parameters as follows. The process and measurement noise variances are estimated to be 2

wσ = 0.01 and 2vσ = 0.001, respectively. The design error covariance is initialized by

solving the design algebraic Riccati equation with 0A = diag (0.9, 0.8, 0.7). The means of the first three parameter estimates, calculated over 20 sets of inertial measurements, are shown in Figure 5. The (1,1), (2,2) and (3,3) components of Ai are indicated by the solid red line, dashed green line and dotted blue line, respectively. It can be seen that the sequences of

Figure 4. Measured, reference and filtered trajectories from a Northrop Grumman LN270 inertial navigation unit. The measured trajectories are indicated by the thin green lines. The reference and filtered trajectories are indicated by the thick black line.

parameter estimates are monotonically nonincreasing. Improved position estimates, obtained as the sum of the filtered error states and the inertial navigation unit measurements, coincided with the reference trajectory, are indicated by the thick black line within Figure 4. This suggests that the estimated state matrix parameters are reasonable.

0 1 2 30

0.2

0.4

0.6

0.8

1

i

a^

i

CONCLUSION This convergence of state matrix and variance estimates within an EM algorithm is investigated. It is established that when the initial estimates of the unknown parameters approach the actual values from above, the sequence of MLEs will be monotonically non-increasing. In the limit, when the measurement noise becomes negligible, the MLEs of the state matrix parameters and process noise variances asymptotically approach the actual values. Conversely, when the process noise is negligible, the MLEs of the measurement noise variances asymptotically approach the actual values. Underground and surface mine navigation applications are discussed. It is demonstrated that inertial measurements can exhibit significant departures from their true trajectories. EM algorithms are outlined which estimate unknown model parameters for recovery of position estimates. REFERENCES McLachlan G. J., Krishnan T. (1997), The EM Algorithm and Extensions, John Wiley & Sons

Inc., New York. Kay S. M. (1993), Fundamentals of Statistical Signal Processing. Estimation Theory.,

Prentice-Hall Inc., Englewood Cliffs, New Jersey.

Figure 5. Mean state matrix parameter estimates versus iteration number. The (1,1), (2,2) and (3,3) components of Ai are indicated by the solid red line, dashed green line and dotted blue line, respectively.

Einicke G. A., Malos J. T., Reid D. C., Hainsworth D. W. (2009), Riccati Equation and EM Algorithm Convergence for Inertial Navigation Alignment, IEEE Trans. Signal Process., vol 57, no. 1.

Savage R. P. (2000), Strapdown Analytics, Strapdown Associates, vols. 1 and 2. Seidelmann P. K. ed. (1992), Explanatory supplement to the Astronomical Almanac, Mill

Valley, Cal., University Science Books, pp. 52 and 698. Einicke G. A., Ralston J. C., Hargrave C. O., Reid D. C., Hainsworth D. H. (2008),

“Longwall Mining Automation. An application of minimum-variance smoothing, IEEE Control System Magazine, vol. 28, no. 6, 28 – 37.