Delayed Fusion of Relative State Measurements by Extending Stochastic Cloning via Direct Kalman Filtering

Delayed Fusion of Relative State Measurements by Extending Stochastic Cloning via Direct Kalman Filtering Ehsan Asadi and Carlo L Bottasso Department of Aerospace Science and echnology Politecnico di Milano, Milano, Italy, 6 ehsanasadi@polimiit and carlobottasso@polimiit Abstract In this work, a real-time localization problem is formulated by incorporating exteroceptive sensors as sources of both relative state measurement and delayed information within a direct Kalman filtering approach he problem of delayed incorporation of relative state measurements is addressed by extending the stochastic cloning framework, whereby an augmented state vector is defined by copies of the current and delayed states he formulation is developed within a tightly coupled visionaided inertial navigation system suitable for GPS-denied environments o enhance performance, a previously described approximate observation model is replaced here with an exact one he proposed formulation is tested and demonstrated with simulation experiments conducted on a small RUAV flying in a virtual environment, showing the mitigation of adverse effects on the estimator performance caused by delays and the nature of relative state measurements Keywords Stochastic cloning; Relative state measurements; Delayed measurements; Vision aided INS I Introduction Self localization via multi sensor fusion is a key technology and an enabler of many tasks in navigation and control of autonomous vehicles In order to achieve high accuracy and robust interaction with dynamic environments, intelligent systems are nowadays equipped with various types of sensors comprising both proprioceptive sensors [] (such as encoders, gyros or accelerometers) and exteroceptive ones (such as sonars, laser scanners or cameras) In contrast to a GPS or a compass, which provide the absolute position or heading of the vehicle, relative information can be acquired by using lasers or stereo cameras yielding the position of the environment features with respect to the vehicle Standard Extended Kalman Filtering (EKF) formulations are typically not adequate for optimally incorporating relative measurements Simultaneous Localization and Mapping (SLAM) methods are capable of handling this situation, but their computational requirements can be significant due to the state vector augmentation that they perform his hinders the use of Visual-SLAM (V-SLAM) approaches in various real-time demanding applications his work is based on a localization approach resulting in an algorithm that is suitable for demanding real-time performance in GPS-denied environments A possible localization approach making use of relative state measurements is based on an approximate observation model using (average) translational and rotational velocities during the measurement acquisition time interval []; this way, measurements can be expressed directly via the current state of the vehicle his approach is accurate when the motion velocity is approximatively constant between steps A more formal approach to this problem was proposed by Ref [3], which developed a stochastic cloning framework for relative state (pose) measurements he formulation of Refs [], [3] was based on the indirect form of the (loosely coupled) Kalman filter, and estimated the errors in the states rather than the states themselves A similar implementation was also reported in Ref [4], which proposed a loosely coupled vision-aided inertial navigation with a delayed state filter to process relative state measurements In vision-based navigation systems [], latency due to the extraction of information from images in real-time applications is the other factor (besides computational complexity) limiting the frequency of fusing visual information By increasing the frequency at which image frames are incorporated into the process, real-time performance is typically achieved at the expense of accuracy by reducing the computational complexity and the number of features From a different point of view, it might be more effective in some cases to avoid reducing the computational complexity, in favor of the delayed fusion of more accurate information at a lower frequency More in general, better performance may be achieved by trade offs between frequency and computational complexity Measurement delay has been the subject of numerous investigations, for example in the context of systems requiring long time visual processing [6], [7] Some methods fuse delayed measurements as they arrive [8], [9]; these methods are effectively implemented in tracking and navigation systems for handling GPS delays he implementation of delayed state filters has also been proposed in several applications [], [] he problem is more complicated if the delayed measurement is also a relative state measurement, as in the case of visual information acquired by tracking feature points in image scenes Goal of this work is to simultaneously mitigate the effects on the estimator performance caused by the nature of relative state measurements and by delays generated by the on-line processing of visual information First, the stochastic cloning

formulation [] is rewritten here in a direct form of Kalman filtering hen, the problem of delayed incorporation of relative state measurements for a real-time application is addressed by extending the state vector with copies of the current and delayed vehicle states his way, the augmented state vector, which includes three copies of current and delayed vehicle states, introduces more interdependent effects into the approach via cross-correlation terms A vision aided inertial navigation approach [] in GPS-denied environments is here optimized by exploiting an exact observation model within the proposed extended stochastic cloning framework Simulation experiments are conducted to assess the performance of the proposed extended stochastic cloning approach in the presence of delayed relative state measurements II Delayed Relative State Measurements he standard EKF estimator is based on the following statespace model: ( ) x k+ = f k xk,µ k, z k = h k (x k )+ξ k, (a) (b) where f ( ) and h( ) express the process and observation models, respectively Furthermore,µ k andξ k are the associated process and measurement noise vectors, respectively, assuming E{µ k µ k }= Q and E{ξ k ξ k }=Υ Consider now z k to be a relative state measurement, logged at time step k, but provided to the estimator with m time steps of latency, noted z k his way one may derive a non-standard observation model that can be expressed as z k = h k+m (x k+m k, x k k, x, z )+ξ k () III Stochastic Cloning Framework Delayed state Kalman filtering is an optimal approach for incorporating delayed measurements where z k = h k+m (x k+m k, x k k )+ξ k An augmented state vector, x k k, is used within the estimator by extending the state vector with a same copy of the vehicle state at time step k, by defining x k k = [ x k k x k k] (3) A more general stochastic cloning framework was presented in an indirect form of Kalman filtering [3] to correctly account for the correlations between consecutive relative-state measurements, where z k+m = h k+m (x k+m k, x k k, z k )+ξ k+m According to Ref [3], the filter (error) state vector was augmented to include two copies of the vehicle error 3 state, x, and a copy of observations his way, at time step k the augmented state is defined as x k k = [ x k k x k k z ] k (4) he star symbol, [ ], indicates a delayed measurement with m time steps of latency he bar symbol, [ ], indicates the augmented form of vectors or matrices 3 he tilde symbol, [ ], is used to signify the error between the actual value of a quantity and its estimate, ie x k k = x k ˆx k k IV Extended Stochastic Cloning Considering that delay may be associated with exteroceptive measurements, the relative state measurement is related to the vehicle states at three different times instead of two, as indicated in Eq () herefore, the previous frameworks represented by delayed state Kalman filtering and stochastic cloning does not in general provide an optimal solution in this case o address this problem, in the present work copy of the current and delayed vehicle states are appended to the current state, alongside with the latest exteroceptive measurements, this way obtaining an extended stochastic cloning framework In comparison to Ref [3], this implementation allows for a larger number of cross-correlation terms, among the vehicle state at three different times and between the vehicle states and the observations Considering the last relative measurements, which were logged at time step k m and fused at k, the proposed augmented state vector is given by the following expression x k k = [ x k k x k k x z ] () In comparison to SLAM approaches, here the state vector and the computational cost do not grow with the environment size, as only frame to frame measurements are incorporated Environment features are directly augmented to the state vector and they are replaced by a new one as soon as they disappear from the field of view A Resetting the Cloned Filter In order to prepare the cloned filter, the augmented state vector and covariance matrix are reset right after completing the Kalman update by the last exteroceptive measurement Considering time step k, the augmented state vector is given by Eq () and the covariance matrix is reset as P k k = P k k P k k P xk x P xk z P k k P k k P xk x P xk z P x k x P x k x P P x z P x k z P x k z P x z P z z (6) Sub-matrices P k k and P in Eq (6) are acquired according to the standard Kalman filter implementation and directly extracted from the covariance matrix before it is reset However, sub-matrices P xk x, P xk z, P x z and P z z are the required unknown blocks at the sample time step k, and should be computed after the Kalman update hese matrices are evaluated later on in Sect IV-D B State Propagation Consider a linearization of the previous system (a), expressed in the discrete form x k+ = F k x k + G k µ k, (7) where F k and G k represent Jacobians of the process model f k with respect to the state x k and noiseµ k vectors hen, the propagation of a non-augmented state vector is obtained as x k+ k = F k x k k, P k+ k = F k P k k F k + G k Q k G k, (8a) (8b)

Subsequently, after m time steps of propagation, the above formulation can be modified as, x k+m k =F k k+m x k k, (9a) P k+m k =F k k+m P k k Fk k+m +Q k k+m, (9b) wheref k k+m = m i= F k+m i andq k k+m = E { σ k k+m σk k+m} is the covariance of σ k k+m, while σ k k+m is the corresponding process noise after m time steps of propagation Further details on the formulation are given in Appendix I Using a similar approach, the corresponding updates for the augmented state vector and covariance matrix can be written as x k+m k =F k k+m x k k, (a) P k+m k =F k k+m P k k F k k+m+q k k+m, (b) where x k+m =F k k+m x k +σ k k+m, and F k k+m Q k k+m F k k+m = I, Q k k+m= his way, after m time steps of propagation, the augmented state vector and covariance matrix at time step k+m are given by x k+m k = [ x k+m k x k k x z ], () P k+m k = P k+m k F k k+m P k k F k k+m P xk x F k k+m P xk z P k k Fk k+m P k k P xk x P xk z P x k x Fk k+m P x k x P P x z P x k z Fk k+m P x k z P x z P z z () According to Eq (), a large block of the covariance matrix will be kept unchanged during state propagation C State Update Consider now time step k+m, when a new delayed measurement is fed to the estimator he Kalman equation at the update step can be applied after computing the augmented covariance of the residual and the Kalman gain: where S= H k+m P k+m k H k+m+υ k, (3) K=P k+m k H k+ms = [ K k+m K k K K z ], (4) H k+m = [ H k+m k+m H k+m k H k+m M ], () and H k+m k+m, H k+m k, H k+m and M k+m represent the Jacobians of the nonlinear observation model, expressed by Eq (), with respect to x k+m, x k, x and z, respectively he main goal is to update the current state of the vehicle, but the delayed states are also required to be updated to prepare the augmented covariance matrix for the next update step, and this is obtained as follows: ˆx k+m k+m = x k+m k + K k+m ( z k ẑz k), (6a) ˆP k+m k+m = P k+m k K k+m S K k+m (6b) D Evaluating the Cross-Covariance Relations In order to complete the above formulation, it is necessary to compute four submatrices, which are unknown components of the augmented covariance matrix expressed by Eq (6) Here the matrices are computed at time step k+m, whereas the corresponding matrices at time step k can be readily extracted by a similar formulation o compute the unknown term P xk+m x k, first the cross covariance between the augmented state vector at two different time steps is expressed wrt the covariance matrix at time k +m P xk+m x k = E{ x k+m k+m x k k} = E{[(I K k+m H k+m ) x k+m k K k+m ξ k ] [F k k+m( x k+m k σ k k+m )] } = (I K k+m H k+m )( E{ x k+m k x k+m k} E{ x k+m k σ k k+m } )F k k+m P k+m k+m F k k+m (I K k+m H k+m )Q k k+m F k k+m, (7) where (I K k+m H k+m )E{ x k+m k x k+m k} is replaced by P k+m k+m, assuming the Kalman gain to be optimal, and E{ x k+m k σ k k+m } E{[F k k+m x k k +σ k k+m ][σ k k+m ] }=Q k k+m Finally, by Eq 7 and using a blockwise inversion off k k+m, block P xk+m x k can be readily expressed as P xk+m x k = (P k+m k+m Q k k+m )Fk k+m, (8) Q k k+m = (I K k+m H k+m k+m )Q k k+m In order to simplify the computation of the other interdependent terms, here the last measurement is assumed as a temporary variable augmented to the state vector, similarly to what done in Ref [3] herefore, we have X= [ x k+m k z k ], (9) H= [ H k+m ] () Noting that P xk+m k z = P k k+m k H k+m, the corresponding covariance ofxcan be written as P P k+m k = k+m k P k+m k H k+m H k+m P k+m k H k+m P k+m k Hk+m+Υ () k Regarding the sparse observation model H, the residual covariancesis equal to S, and the corresponding Kalman gain is expressed as K=PH S = [K K z k+m ] = [K (H K) ] () Finally, by applying the Kalman update, the covariance matrix is given as P k+m k+m P xk+m z P k+m k+m = k =P k+m k KSK = P x k+m z k P k+m k K S K H k+m (P k+m k K S K ) P z k z k (P k+m k K S K )H k+m H k+m (P k+m k K S K )H k+m+υ k (3)

According to Eq (3), and noting that P k+m k+m = P k+m k K S K, the cross-covariance terms P xk+m z, P x k k z and P z k k z k at time step k+ m can be readily expressed as P xk+m z k = AP k+m k+m H k+m ; A=[ I ], (4a) P xk z k = BP k+m k+m H k+m ; B=[ I ], (4b) P z k z k = H k+m P k+m k+m H k+m+υ k (4c) A similar formulation can also be derived for the crosscovariance terms at time step k P xk z, P x z and P z z, previously introduced in Eqs (6) and () his way, the augmented covariance matrix after m time step of propagation, as expressed by Eq (), can be rewritten as Eq () Considering the expression of Eq (), in order to have an efficient implementation in terms of computational cost, it is advisable to accumulate the products off k k+m andf k, where the required multiplications to extract P k+m k are delayed and carried out when the new delayed measurements arrive at the estimator V Case Study A tightly-coupled vision aided inertial navigation system (VA-INS) for GPS-denied environments was proposed in Ref [] and subsequently extended in Ref [7] A set of environment features tracked by the vision system were treated as independent visual measurements, and they were fused with inertial readings in order to enhance the estimates in the case of GPS unavailability However, an approximate observation model was used to handle relative state measurements Goal of this work is to improve on the previous formulation by optimizing the estimator performance in real-time applications o this end, an exact vision-based observation model and the delays associated with visual measurements are optimally considered here through the proposed extended stochastic cloning framework he estimator is based on a state vector composed of translational and angular velocities, v and ω, together with the position, r, and quaternion parameters, q, ie x v (v,ω, r, q ) (6) he VA-INS architecture investigated here is based on tracking scene points among stereo images and across time steps Consider a fixed feature point P, whose position vectors wrt framescandc of a stereo vision setup are noted d and d, respectively hus, an exact feature-based observation model discretized across two consecutive time instants t and t k, is given by d (t k ) C k = C ( R(t k ) (r(t k ) r(t ))+ (I R(t k ) R(t ))(c+ Cd(t ) C ) ) + d(t ) C, (7) where matrix C indicates the constant-in-time rotation that brings the body-attached frame intoc, and c is the position vector of point C with respect to the origin of the body frame Furthermore, R = R(q) are the components of the rotation tensor that brings the fixed Earth frame into the body frame Assuming m time steps of latency, the left hand side of Eq (7) is provided by the vision system at time k+ m Equation (7) also represents the relation of the position of a feature point wrt the camera at time t k, noted d(t k ) C k, to its position at t, noted d(t ) C, through the partial vehicle states (position and quaternions) at times t k and t According to the proposed extended stochastic cloning framework, the augmented state vector for this specific problem is defined here as where x d (r, q ) x k k = [ x v k k x d k k x d z ], (8) VI Experiments and Results In this simulation experiment, a small helicopter performs a low level flight in an urban canyon within a small village he stereo camera is mounted in front of the vehicle looking forward to buildings and several other objects featuring realistic textures A Matlab/Simulink simulator was developed, that includes a flight mechanics model of a small RUAV, models of inertial navigation sensors, magnetometer, GPS and their noise models he simulator is used in conjunction with the OGRE graphics engine [], for rendering a virtual environment scene and simulating the image acquisition process All sensor measurements are simulated (see able I) as the helicopter flies at an altitude of m following a rectangular path at a constant speed of m/sec (see Fig ) Navigation measurements are provided at a rate of Hz, while stereo images at the rate of Hz with a sec delay he sonar altimeter is also available at Hz he GPS, available at a rate of Hz, is turned off after sec in the flight, to demonstrate entering into a GPS-denied region In order to have a comprehensive assessment of the proposed formulation, various experiments are conducted for the two conditions of relative state measurements without and with delay ABLE I: Sensors and vibration noise levels Sensors Noise Level Gyro deg/s Accelerometer m/s Magnetometer 4 Gauss GPS m Fig : Simulated small village environment and flight trajectory

P k+m k = P k+m k F k k+m P k k F k k+m (P k k Q k )F P k k F k k+m P k k (P k k Q k )F k F k (P k k Q k ) F k k+m k F k k+m AP k k H k AP k k H k F k (P k k Q k ) P BP k k H k H k P k k A F k k+m H k P k k A H k P k k B H k P k k H k+υ () ) Relative state measurement with no delay Figures (a)-(f) report the estimated trajectory and position errors, in the presence of no delay, using different approaches: Approximate approach he approximate observation model is used, where the relative displacement and rotation of the vehicle are computed based on (average) translational and rotational velocity states Delayed State Kalman filtering (DSK) he time correlation between measurement noises are neglected, ie x k k = [ x k k x k k] Direct Stochastic Cloning (DSC) he simple form of the proposed formulation via direct Kalman filtering is used, where the corresponding parts of formulation that address latency are removed, ie x k k = [ x k k x k k z ] ) Relative state measurement in the presence of delay Figures 3(a)-3(f) report the estimated trajectory and position errors, in the presence of delay, using different approaches: Delayed State Kalman filtering (DSK) his approach is used to demonstrate the effects when delay is not properly handled within the estimator, using x k k = [ x k k x k k] Simplified form of Extended Direct Stochastic Cloning (S-EDSC) he simple form of the proposed extended stochastic cloning is used, where the corresponding parts of the formulation that address the time correlation between measurement noises are removed, ie x k k = [ x k k x k k x ] Extended Direct Stochastic Cloning (EDSC) he complete form of the proposed extended stochastic cloning is used, ie x k k = [ x k k x k k x z ] Figures 4 and report the velocity analysis for each approach, without and with considering delays, respectively he loss of the GPS signal may cause a rapid divergence of the estimates of the standard non-vision-aided observer, however the vision-aided navigation system is capable of enhancing the estimates even in the absence of GPS readings According to the results in the case of no delay, using the exact vision-based model allows for a considerable improvement of the estimates in comparison to the approximate model via standard EKF, cf Figs (a)-(f) Furthermore using a direct stochastic cloning approach provides for smoother estimates, where the filter bounds are also better fitted to the position errors his improvement is clearer in the position rather than the velocity estimates Moreover, the results show the unsuccessful performance of the standard form of the delayed state Kalman filtering and of stochastic cloning for handling both delay and relativestate issues simultaneously On the other hand, the proposed extended stochastic cloning achieves a considerable improvement of the estimates in the presence of delayed relative state measurements, as shown in Figs 3(a)-3(f) VII Conclusion In this work, a localization problem through direct Kalman filtering has been studied, by incorporating exteroceptive sensors as a simultaneous source of relative state and delayed measurements he stochastic cloning framework was extended for a direct form of Kalman filtering, in which the problem of delayed incorporation of relative state measurements is also addressed It was proposed to extend the state vector with a copy of the current and of the delayed vehicle states, this way introducing more interdependencies in the model he formulation was used to optimize a tightly coupled vision-aided navigation system operating in GPS-denied environments A previously described approximated observation model was replaced with an exact one, with the purpose of mitigating effects on the estimator performance caused by delays and by the nature of relative state measurements he proposed formulation was tested and demonstrated with simulation experiments conducted on a small RUAV flying in a virtual environment he results show the rather disappointing performance of standard delayed state Kalman filtering and stochastic cloning for handling both delay and relative-state simultaneously On the other hand, the proposed extended stochastic cloning approach allows for a considerable improvement of the estimates in the presence of relative state measurements associated with delay σ k k+m = k+m j=k+ VIII Appendix I k+m j F k+m i G j µ j + G k+m µ k+m i= (9) Q k k+m = E { σ k k+m σk k+m} = k+m k+m j k+m F k+m i G j j Q j G j F k+m i j=k+ i= i= + G k+m Q k+m G k+m (3)

6 4 3 6 4 3 3 4 6 7 8 9 (b) Approximate Approach Position errors (a) Approximate Approach rajectory estimate; GPS is off after sec 4 3 6 4 3 3 4 6 7 8 9 8 9 (d) DSK Position errors (c) DSK rajectory estimate; GPS is off after sec 4 3 4 3 (e) DSC rajectory estimate; GPS is off after sec 3 4 6 7 (f) DSC Position errors Fig : No delay Left: rajectory estimate and Right: Position Errors, with 9% confidence bounds Green line: Error between position estimate and position prediction Red line: Error between position estimate and real position

4 3 4 3 3 4 6 7 8 9 8 9 8 9 (b) DSK Position errors (a) DSK rajectory estimate; GPS is off after sec 4 3 6 4 3 3 4 6 7 (d) S-EDSC Position errors (c) S-EDSC rajectory estimate; GPS is off after sec 4 3 4 3 (e) EDSC rajectory estimate; GPS is off after sec 3 4 6 7 (f) EDSC Position errors Fig 3: In the presence of delay Left: rajectory estimate, and Right: Position Errors, with 9% confidence bounds Green line: Error between position estimate and position prediction Red line: Error between position estimate and real position

3 4 6 7 8 9 3 4 6 7 8 9 3 4 6 7 8 9 Fig 4: No delay Velocity errors, with 9% confidence bounds Left: Approximate Approach, Center: DSK, Right: DSC Green line: Error between velocity estimate and velocity prediction Red line: Error between velocity estimate and real velocity 3 4 6 7 8 9 3 4 6 7 8 9 3 4 6 7 8 9 Fig : In the presence of delay Velocity errors, with 9% confidence bounds Left: DSK, Center: S-EDSC, Right: EDSC Green line: Error between velocity estimate and velocity prediction Red line: Error between velocity estimate and real velocity References [] A I Mourikis and S I Roumeliotis, On the treatment of relative pose measurements for mobile robot localization, in Proc IEEE Int Conf on Robotics and Automation, Orlando, FL, 6, pp 77 84 [] C L Bottasso and D Leonello, Vision-aided inertial navigation by sensor fusion for an autonomous rotorcraft vehicle, in Proc AHS Int Specialists Meeting on Unmanned Rotorcraft, Arizona, USA,, pp 8 [3] A I Mourikis, S I Roumeliotis, and J W Burdick, Sc-kf mobile robot localization: A stochastic cloning-kalman filter for processing relative-state measurements, IEEE ransactions on Robotics, vol 3, no 4, pp 77 73, 7 [4] J P ardif, M George, M Laverne, A Kelly, and A Stentz, A new approach to vision-aided inertial navigation, in Proc IEEE/RSJ International Conference on Intelligent Robots and Systems, aipei, aiwan,, pp 46 468 [] E S Jones and S Soatto, Visual-inertial navigation, mapping and localization: A scalable real-time causal approach, he International Journal of Robotics Research, vol 3, no 4, pp 47 43, [6] S Pornsarayouth and M Wongsaisuwan, Sensor fusion of delay and non-delay signal using Kalman filter with moving covariance, in Proc IEEE International Conference on Robotics and Biomimetics, 9 [7] E Asadi and C L Bottasso, Handling delayed fusion in visionaugmented inertial navigation, in Proc 9th International Conference on Informatics in Control, Automation and Robotics, Rome, Italy,, pp 34 334 [8] H L Alexander, State estimation for distributed systems with sensing delay, in SPIE, 99, pp 3 [9] D Larsen, N A Andersen, O Ravn, and N Poulsen, Incorporation of time delayed measurements in a discrete-time Kalman filter, in Proc 37th IEEE Conference on Decision and Control, 998, pp 397 3977 [] A Gopalakrishnan, N Kaisare, and S Narasimhan, Incorporating delayed and infrequent measurements in extended Kalman filter based nonlinear state estimation, Journal of Process Control, vol, no, pp 9 9, [] M J Stanway, Delayed-state sigma point Kalman filters for underwater navigation, in Proc IEEE/OES Autonomous Underwater Vehicles (AUV), Woods Hole, MA, USA,, pp 9 [] G Junker, Pro OGRE 3D Programming New York: Springer-Verlag, 6