Closed Form Solutions to the Multiple Platform

Closed Form Solutions to the Multiple Platform Simultaneous Localisation and Map Building (SLAM) Problem Eric W. Nettleton, Peter W. Gibbens and Hugh F. Durrant-Whyte Australian Centre for Field Robotics Department of Aeronautical, Mechanical and Mechatronic Engineering The University of Sydney, NSW 2006, Australia

ABSTRACT

This paper presents a closed form solution to the multiple platform simultaneous localisation and map building (SLAM) problem. Closed form solutions are presented in both state space and information based forms. A key conclusion of this paper is that the information-state based form oers many advantages over the state space formulation in allowing the SLAM algorithm to be decentralised across multiple platforms. The bene ts of operating SLAM in an information form are numerous. The additive properties of the information update make it especially attractive, as does the ability to predict estimates through any direction in time. However, of paramount importance is the well-known ability to decentralise the information lter. A general form of the continuous time inverse covariance matrix for the SLAM problem is presented to determine such properties as the initial and steady state conditions. These properties are investigated to determine their dependence and relationship to both the observation and process noise. Examination of the structure of the general form of the inverse covariance matrix also gives an insight into what information should be communicated between platforms in the decentralised architecture and how it can be managed. Keywords: SLAM, Information Filter, Multiple Platform

1. INTRODUCTION

The simultaneous localisation and map building problem is of great consequence to robotics and autonomous navigation in general. To place a platform at an unknown location in an unknown environment, and then have that platform build a map of the environment and use it for localisation would make it truly autonomous. A large amount of research has been done on the SLAM problem in previous years.1{5 There has been particular interest and progress in areas such as large scale SLAM problems6 and convergence properties.7,8 However, to date, all of this research has concentrated on traditional state space approaches. This paper looks at the SLAM problem in information space form. The reason for this approach is due to the well known ability to distribute and decentralise the information lter.9 The general focus of this research is to develop algorithms capable of being eciently decentralised across multiple platforms. The bene ts of developing multiple platform SLAM algorithms are enormous. For a single platform, using a SLAM algorithm to decrease its location covariance, it must revisit features it has seen previously. However, with multiple platforms it would be possible to share observations and map information to generate a more accurate map and localisation estimate. Indeed, if certain platforms were directed to revisit shared known features, then there would be signi cant location estimate improvements for all platforms. There are obviously a large number of practical hurdles to overcome for such algorithms to be implemented. The problem of data association of features between platforms will need to be addressed as will the communications strategy.10 The latter will undoubtedly arise as there will be a practical bandwidth limitation for inter-platform communication. However none of these issues are thought to be insurmountable. For the purposes of this paper, all platforms are constrained to one degree of freedom Brownian motion. Platforms make linear observations to stationary scalar features. While these restrictions were applied to simplify this initial Further author information: E.W.N.: E-mail: [email protected]

research, the results give an insight into the structure of the SLAM problem for the more general two and three degree of freedom systems. The information matrix for single platform case is presented initially, then extended to the multiple platform scenario.

2. PROBLEM FORMULATION

This research formulates the SLAM problem in continuous time in order to obtain a closed form solution to the problem. In practice however, the lter will usually be implemented in discrete time.1

2.1. Platform Models

All platforms i = 1; ::; m in this research use the same linear one degree of freedom model. A platform i moves with a velocity measured as ui (t). The errors in these measurements, denoted wi , are assumed Gaussian with variance Qi = E [wi ; wiT ] = qi . The model of a platform i can therefore be described by

x_ i (t) = ui (t) + wi

(1)

The features j = i; :::; n are located throughout the environment at xed locations pj such that p_j = 0. An augmented state vector containing the estimates of the states of all platforms and the feature locations is used, written in the form x(t) = [x1 (t); :::; xm (t); p1 ; :::; pn ]T . The state transition equation then becomes x_ (t) = F x(t) + G(u(t) + w(t))

01 BB0 where F = 0 and G = B B@ .. .

0 1

(2)




0 0





01T

.. . . . . .. . 0





1

.. . . .. C . . .C C .. . . . .. C . .A 0





0

Observations are made of the range to each feature using a suitable sensor on each platform. An observation of feature

j from platform i is written zij (t) = pj ; xi (t) + vj where vj is the observation error and is assumed Gaussian with variance rj . The total observation noise can then be constructed in the form R = diag(r1 ; :::; rn ). The observation

model is written

z(t) = H x(t) + v

(3)

where z(t) = [z11 (t); :::; z1n (t); z21 (t); :::; zmn (t)]T and v = [v11 ; :::; v1n ; v21 ; :::; vmn ]T and

0;1 0


0 BB ... ...


... BB;1 0


0 BB 0 ;1


0 BB . . . H =B BB 0.. ;..1





0.. BB BB 0 0


;1 BB . . @ .. ..


... 0

1 .. . 0 1 .. . 0




01 . . . .. C .C C


1C C


0C C . . . .. C .C C


1C C

CC

(4)

1


0C C .. . . .. C . . .A 0


;1 0


1

The combined state estimate for this system is de ned as x^(t) = E [x(t)jZt ]. The variance is de ned as the mean squared error in this estimate, and is written P (t) = E [x2 (t)jZt ], where x(t) = x(t) ; x^ . In information space, the inverse of the state variance is maintained, and is denoted Y (t) = P ;1 (t).

2.2. Solutions to the Riccati Equation

The SLAM covariance matrix is determined by solving the algebraic Riccati equation, shown in Equation 5. Similarly, the information matrix is the solution to the inverse of the algebraic Riccati equation given by Equation 6. The interest of this research focuses on the evolution of this combined state covariance matrix. P_ (t) = FP (t) + P (t)F T + GQGT ; P (t)H T R;1 HP (t) (5)

Y_ (t) = ;Y (t)F ; F T Y (t) ; Y (t)GQGT Y (t) + H T R;1 H

(6)

There a numerous known methods for solving this non-linear equation.11 The method used in this work was to represent the Riccati equation as a fraction decomposition in the form

P (t) = U (t)V ;1 (t)

(7)

Y (t) = V (t)U ;1 (t)

(8)

for a state space solution, and for an information space solution. The solution to the Riccati equation can now be obtained from the solution to the linear dierential equation12  U_   F  U  Q (9) V_ = HR;1 H ;F V where the matrices U(t) and V(t) are solutions and have initial conditions

 U (0)  P (0) = V (0)

(10)

I

The transition matrix of Equation 9 is known as the Hamiltonian. In this development, the Hamiltonian can be further simpli ed because F = 0.

2.3. General Solution: n Features m Platforms

The solutions shown in this paper were obtained using a commercial symbolic algebra package for n = 1; 2; 3 features and m = 1; 2; 3 platforms. Using these solutions, the general case was deduced and checked by substitution back into the Riccati equation.

3. SINGLE PLATFORM SLAM

This problem involves a single platform using n features for SLAM. Each feature is assumed to be continuously observable. Data association is not considered in this problem as all observations are always correctly matched. All features are point targets and are viewpoint invariant. These assumptions are made in order to simplify the problem and allow investigation of the structure of the continuous time SLAM information matrix. The vehicle uses a constant velocity model and all features are stationary. The SLAM information matrix solution for the single platform case is shown in Appendix A. The diagonal feature j information is expressed as (1 ; e;t ) Yjj (0) + rt ; r2tI + r22 I (1 (11) j j T j T + e;t )

p

P

where  = qIT is the dominant time constant for the system and IT = Ni=1 ri;1 is the total Fisher information available to the lter. This problem is formulated with the initial condition diag( r11 ; :::; r1n ). As long as Y(0) is diagonal, the constant term of Yjj (t) is the initial condition Yjj (0). Some work has been done on the more general case of non-zero o diagonal initial conditions, but it is not complete at time of publication.

The second term is rti . In continuous time, r1i can be thought of as the amount of information per second added to the system by observing feature i. Using this description, the term rti simply becomes the amount of information added after observing the feature i for t seconds. The remaining terms describe the diusion of information throughout the map. The o diagonal feature i to feature j cross-information is structured very closely to that of the diagonal elements.

; e;t ) ; r rt I + r 2r I (1 i j T i j T (1 + e;t )

(12)

The primary dierence is that the o diagonal terms do not contain the rti term described above. This is to be expected as this term relates to a continuous observation of a feature rather than the cross information between features. Another point of note is that the solution to this problem was obtained with zero initial conditions for the nondiagonal terms. This resulted in the solution having no constant terms o the diagonal. The feature i to platform cross information is in the form (1 ; e;t) ; r1 (1 + e;t) i

(13)

The cross information for a feature i is clearly dependent on ri . This diers signi cantly from the state space case where all cross-correlations are the same.8 In the limit t ! 1, the platform to platform information approaches q . The limits of the vehicle to map components are dierent for each feature, and are governed by ri for that feature. This diers from the state space equivalent where the feature to feature and feature to map terms were all equivalent in the limit.8 Also, the information term for a particular feature grows without bound. This is reasonable because as the platform continuously observes the feature, it is always accumulating more information about it. Similarly, the cross information between features is unbounded.

4. MULTIPLE PLATFORM SLAM

Multiple platforms are now considered, where each platform observes the same n continuously observable, viewpoint invariant features at all times. These features are used for SLAM. Data association is not considered in this problem and platforms only sense the features and not the relative location of each other. The process noise q for platform i is denoted qi . The information matrix for the SLAM problem is shown in Appendix B, while Appendix C contains the covariance matrix for the same problem. Note that in the formulation of the state space problem of Appendix C, the problem was simpli ed by assuming that all platforms have the same process noise q. As each platform moves along sensing features, it shares its information with all other platforms which in turn use that information to update their own map and location. That is, this problem is constructed to simulate a fully connected network of platforms sharing their observations.

4.1. State Space Solution

The characteristic equation of the system8 with m platforms is expressed as

D(t) = ( + m) + ( ; m)e;2t

(14)

This is dependent on the number of platforms. This occurs as all platforms are correlated through the use of the same map, and subsequently aect one another. The diagonal platform to platform elements for multiple vehicles are in the form

q(1 ; e;2t ) + a + (m ; 1)b D(t)

(15)

2q

;t

q

;2t

;e ) . This term is very similar to the equivalent single platform term.8 The where a =  (1D;(et) ) and b = D((1t)(1+ e;2t ) dierence between the two solutions is the addition of the (m ; 1)b term in this multiple platforms case. However, this occurs as the single platform solution is a special case where (m ; 1) is clearly zero. The structure of the equation is somewhat simpli ed due to each platform having the same value q. The term beginning (m ; 1) q would certainly change if q were dierent for each platform as it would no longer be a multiple of the same value q. However, it is important to note that the platform to platform cross correlations are all non-zero and equal, as each platform is correlated through the use of the common map. The platform i to feature j cross correlations are expressed as 2

2

q ;t 2  (1 ; e )

D(t)

(16)

and are all identical under the assumption q is the same for all vehicles. The extension from the single to the multiple platform case changes the map to map elements only slightly. The feature i covariance is expressed as

ri (IT ; ri;1 ) + q (1 + e;2t ) IT (mt + 1) D(t)

(17)

while the feature i to feature j cross correlation is q ;2t )  (1 + e + (18) IT (mt + 1) D(t) The time term t on the denominator of the rst term is multiplied by the number of platforms m. This occurs as all platforms, using the same process noise q, are observing the same features with the same observation noise r. The net result is that this can be thought of as one sensor observing the same features for m times as long (where m is the number of platforms). This is the only change to the elements in the multiple platform case. Note also that this solution was obtained with the initial condition of the map sub-matrix YMM (0) = R. In the limit t ! 1, the platform to platform cross correlations become equal to the platform to map cross correlations and also the map to map cross correlations. The reason for this is that everything, including the dierent platforms, is correlated through the use of the same map.

;1

4.2. Information Space Solution

The diagonal elements of the platform to platform sub-matrix (platform i information) are

i (1 + e;2i t ) qi (1 ; e;2i t )

(19)

and are clearly the same as for the single platform case shown in Equation 23. All o-diagonal elements (platform i to platform j cross information) are zero. Simulation has also con rmed this result. Although the assumption was made that the platforms do not sense each other, they will be correlated in state space as they are navigating using the same map. The state space platform to platform cross correlations are clear in Equation 34 as the matrix is fully populated. In information space the platforms information vectors are constructed to be orthogonal to one another, which results in the cross information being zero. For a more complex system where each platform has multiple states, the full platform to platform matrix should be block diagonal. This could be very advantageous in communications management as all platform to platform cross information need not be transmitted. The platform i to feature j cross information is in the form ;(1 ; e;i t ) (20) rj i (1 + e;i t ) which is also the same as for the single platform case. This arises as all platform information vectors are orthogonal to one another, and therefore the platform to map cross information from one platform will not contribute to another.

All elements in the feature to feature sub-matrix (the map sub-matrix) are simply the sum of the contributions from each platform, plus the initial condition. This can be seen as the feature i information is

Yii (0) +

m t(I ; r; ) b X [ T i + k] 1

k=1

ri IT

ri2

(21)

and the feature i to feature j cross information is

m ;t X [

bk ] + k=1 ri rj IT ri rj

(22)

; t

;e k ) where bk = k2(1 IT (1+e;k t ) . This follows logically from the structure of the information lter update stage which is additive.9 The limits of the feature to feature elements are all unbounded as t ! 1. This occurs as extra information is continually being added to the map information matrix.

5. DISCUSSION

Looking at the structure of the multiple platform solutions presented in this paper indicates that working in information space has a number of advantages over state space. The additive nature of the information update is particularly attractive, and is the reason why in information space the total map is simply the sum of the contributions from each individual platform. The fact that the platform to platform cross information is zero could be very useful in developing a communications strategy.10 At worst, if a full SLAM solution was implemented, the number of terms to communicate would be reduced. As long as the platforms do not sense each other, then there is no need for one platform to communicate the cross information between platforms as they should all remain zero. In the more complex case where each platform has more than one degree of freedom and hence more states, the platform to platform sub-matrix should be block diagonal as the platform states still remain orthogonal to those of other platforms. While there are advantages of operating SLAM in information space, there are a number of drawbacks as well. While the information lter is numerically equivalent to the Kalman lter, its representation of the problem is signi cantly dierent. In the SLAM case, this means that there is no actual \map" being generated, rather, the inverse of a map. This means that an inversion will always be required if any physical location needs to be extracted. Another potential problem with operating in information space is the map management issue. It is not possible to delete a row/column in information space, as the o diagonal terms aect the inversion. However, it can be done by inverting back to state space, performing the required calculations, and returning to information space. This is another area which requires further research. One of the main areas in which the SLAM algorithm diers from the Kalman lter, is that of the limits. In a standard linear Kalman lter, it has been shown that in the limit the lter is independent of its initial conditions. However, this is clearly not the case in SLAM as is evidenced by the initial condition appearing as a constant in the information matrix. It has also been shown practically that the initial condition will aect the limit. The SLAM problem is highly dependent on the Fisher information available to the lter, given by Equations 25, 32 and 40. This is reasonable as it represents the total information available to the lter per unit time - a value which will logically aect the accuracy of the solution. The platforms process noise qi is also of importance as it represents the degradation of the solution due to uncertainty in the platform. Both of these values are used to determine the systems dominant time constant , given by Equations 24, 31 and 39. This time constant represents the ratio of total information loss to total information gain.8 It is important to note that in the multiple platform case shown in Appendix B, each platform i has an independent value of , denoted i . In the platform to platform and platform to map sub-matrices shown in Equations 27 and 28 respectively, the information at each element is independent of the process noise and time constant of all other platforms. It is only in the map (the feature to feature sub-matrix of Equation 29) that the information on platform i, is aected by platform j .

6. CONCLUSIONS

This paper has presented a continuous time, closed form solution to both the single and multiple platform SLAM problems in information space as well as the multiple platforms problem in state space. These solutions were investigated and analysed in order to understand the structure of the problem and to provide an insight into how the goal of multiple platform SLAM can be practically applied in two and three dimensions. In comparing the state space and information space solutions to the multiple platform SLAM problem, the latter seems to oer a number of advantages.1 It is clear that in information space, the multiple platform solution follows logically from the single platform case. All terms in the multiple platform solution can be readily derived from the equivalent single platform solution. In contrast to this, the state space solution has extra terms in the multiple platform solution that were not present for the single platform case. The inverse covariance solution also clearly adheres to the standard information lter update stage as new information is merely added to the system. This is most evident in the map component of the information matrix as it is clear that in information space, the total map information is simply the sum of the map information contributed from each platform. The fact that the cross information between platforms is zero in information space has a number of implications for communications management. At the very least it reduces the number of terms to communicate in the full SLAM implementation. However, it may potentially be exploited further. A great deal more research is still needed to solve the multiple platform SLAM problem. The data association and communications between platforms will need addressing, in addition to extending the problem to two and three degrees of freedom. However, the results presented thus far strongly support the need for continuing this work, and suggest that representing the problem in information space is particularly bene cial.

APPENDIX A. INVERSE COVARIANCE MATRIX - SINGLE PLATFORM INFORMATION SPACE

The inverse covariance matrix Y (t) for general form of the n feature scalar SLAM problem

0  e; t


; ri  ;ee;;tt


q ;e; t B B . . . .. .. .. B B ;t t B ; e ;e;;t t t ;


Y (0) + ;


+ B ; t ii r  e r e r I r I i i i T i T B .. .. B ... B . ;t . @ ; t ;e ;e t (1+ (1

1

; rj  1

2 2

) )

(1 (1+

1

(1 (1+

) )

(1 (1+

e;t )

)

(1 (1+

2

2






) )

2

; ri rj IT + ri rj IT 2

(1 (1+

) )

p

 = qIT and

IT = is the total Fisher information.

N X i=1

1

t ) ;e;;t

(1 (1+

.. .

e

; ri rtj IT + ri rj IT 2

.. .

)

1 CC CC t ;e;;t CC e CC C ;e;t A

(1 (1+




Yjj (0) + rtj ; rj2tIT + rj2 IT

)

e;t )

Where

; rj 

ri;1

2

) )

(1 (1+

(23)

)

e;t )

(24) (25)

APPENDIX B. INVERSE COVARIANCE MATRIX - MULTIPLE PLATFORMS INFORMATION SPACE

Inverse covariance matrix Y (t) for general form of the n feature, m platform scalar SLAM problem. Process noise is for platform i is qi . The matrix is decomposed into three dierent sub-matrices. These are platform to platform (PP), platform to map (PM) and map to map (MM) components. The structure of the full matrix is shown below.

 PP PM  T PM

(26)

MM

The vehicle to vehicle (PP) component.

0 BB q BB BB @

1 (1+ 1 (1

e;21 t )

;e;21 t )

0 .. . 0






0

.. . ...

2 (1+e;22 t ) q2 (1;e;22 t )











0

0 0 .. .

m (1+e;2m t ) qm (1;e;2m t )

1 CC CC CC A

The vehicle to map (PM) component.

0 ; BB r  BB r ; BB @

;e;1 t )

;e;;2t )t






2 )

;(1;e;1 t ) r2 1 (1+e;1 t ) ;(1;e;2 t ) r2 2 (1+e;2 t )

;(1;e;;mt )t

;(1;e;;mt )t






(1 1 1 (1+ (1

1 2 (1+

.. .

e;1 t ) e

r1 m (1+e m

)

.. . ...






r2 m (1+e m

;(1;e;1 t ) rn 1 (1+e;1 t ) ;(1;e;2 t ) rn 2 (1+e;2 t )

)

.. .

;(1;e;m t ) rn m (1+e;m t )

(27)

1 CC CC CC A

(28)

The map to map (MM) component.

0 Pm [ ;t + bi ] 1 P [ t IT ;r; + bi ] Pm [ ;t + bi ]


Y (0) + m i r rn IT r rn i i r IT r r IT r r r CC B ; . B P P P m [ t IT ;r + bi ] .. m [ ;t + bi ] m [ ;t + bi ] CC B Y (0) + i r r IT r r i i r rn IT r rn r IT r B CC B . . . B .. ..


@ Pm .. Pm [ ;t + bi ] Pm [ t IT ;rn; + bi ]A ;t + bi ] [


Y (0) + nn i rn r IT rn r i rn r IT rn r i rn IT rn (

11

=1

1

1 1 )

=1

2 1

2 1

=1

1

1

2 1

=1

1 2

(

22

=1

2

1 2 1 2 )

2 2

=1

1

1

=1

2

2

(

Where

=1

2

2

;i t

;e ) bi =  2(1 I (1 + e;i t ) i T

p

i = qi IT IT = m=Number of platforms and n=number of features

n X i=1

ri;1

=1

1)

(29)

2

(30) (31) (32)

APPENDIX C. COVARIANCE MATRIX - MULTIPLE PLATFORMS STATE SPACE

Covariance matrix P (t) for the general form of the n feature, m platform scalar SLAM problem. Process noise q is the same for all platforms. The matrix is decomposed into 3 dierent sub-matrices. These are platform to platform (PP), platform to map (PM) and map to map (MM) components. The structure of the full matrix is shown in Equation 33.

 PP

PM PM T MM



(33)

The platform to platform (PP) component.

0q B B B B B B @

(1

;e;2t ) + a + (m ; 1)b D(t)

a;b

a;b


; 2 t q(1;e ) + a + (m ; 1)b ... D(t)

a;b






.. .

The platform to map (PM) component.

0 q BB BB BB @

;e;t )2

(1

D(t)









.. ... . .. ... . ;t 2

q  (1;e

D(t)

)









The map to map (MM) component.

0 r IT ;r; IT mt B B ; B IT mt B B B @

q

;2t

q

.. .

...




a;b

D(t)

.. . .. . ;t 2

D(t)

)

1 CC CC CC A

;2t

(35)

q

;2t

1

) ;1 +  (1+e )


;1  (1+e +  (1+De(t) ) IT (mt ( +1) +1) D(t) IT (mt+1) + D(t) C q ;2t ) r2 (IT ;r2;1 ) q (1+e;2t ) .. 1 ;1 + q (1+e;2t ) C  (1+e CC + + . ( +1) D(t) IT (mt+1) D(t) IT (mt+1) D(t) C .. .. ... . q ;2t


. q ;2t C A rn (IT ;rn;1 ) +  (1+e ) ;1 +  (1+e )





IT (mt+1) D(t) IT (mt+1) D(t)

1(

Where

1 1 )

(34)

q(1;e;2t ) + a + (m ; 1)b D(t)

q ;t 2  (1;e )

q  (1;e

1 CC CC CC A

a;b a;b

q

(36)

(1 ; e;t )2 a =  D(t)

(37)

q (1 ; e;2t )2 b = D(t)(1 + e;2t )

(38)

2

p

 = qIT IT =

N X i=1

ri;1

D(t) = ( + m) + ( ; m)e;2t m=Number of platforms and n=number of features

(39) (40) (41)

ACKNOWLEDGMENTS

This work is funded by BAE SYSTEMS UK and BAE SYSTEMS Australia. The rst author is funded by BAE SYSTEMS Australia and would like to thank them for his PhD scholarship.

REFERENCES

1. R. Deaves, \Covariance bounds for augmented state kalman lter application," Electronic Letters, 11th November 1999, 35, No 23, p. 2062, 1999. 2. R. Smith and P. Cheeseman, \On the representation of spatial uncertainty," Int J. Robotics Research 5, No 4, pp. 56{68, 1987. 3. J.J. Leonard and H.F. Durrant-Whyte, \Simultaneous map building and localisation for an autonomous mobile robot," in IEEE/RSJ International Workshop on Intelligent Robots and Systems, pp. 1442{1447, (New York), 1991. 4. W. Rencken, \Concurrent localisation and map building for mobile robots using ultrasonic sensors," in Proceedings of the 1993 IEEE/RSJ International Conference on Intelligent Robots and Systems, vol. 3, pp. 2192{7, 1993. 5. J.A. Castellanos, J.M.M. Montiel and J. Tardos, \Sensor in uence in the performance of simultaneous mobile robot localization and map building," in 6th International Symposium on Experimental Robotics, pp. 203{212, (Sydney), 1999. 6. J.J. Leonard and H. Feder, \Stochastic mapping using forward looking sonar," in FSR '99 International Conference on Field and Service Robotics, pp. 69{74, (Pittsburg), 1999. 7. M.W.M.G. Dissanayake, P. Newman, H.F. Durrant-Whyte, S. Clark and M. Csorba, \An experimental and theoretical investigation into simultaneous localisation and map building," in 6th International Symposium on Experimental Robotics, pp. 171{180, (Sydney), 1999. 8. P.W. Gibbens, A Closed Form Solution to the Simultaneous Localisation and Map Buliding (SLAM) Problem, ACFR Technical Report 2000/1, Sydney University, 2000. 9. S. Grime and H.F. Durrant-Whyte, \Data fusion in decentralized sensor networks," Control Engineering Practice 2, No 5, pp. 849{863, 1994. 10. R. Deaves, The Management of Communications in Decentralised Bayesian Data Fusion Systems, Ph.D Thesis, Bristol University, Dept of Electrical and Electronic Engineering, 1999. 11. B. Anderson and J. Moore, Optimal Control, Prentice Hall International, 1991. 12. M.S. Grewal and A.P. Andrews, Kalman Filtering Theory and Practice, Prentice Hall, New Jersey, 1993.