IROS2019国际学术会议论文集 1557

Covariance Pre Integration for Delayed Measurements in Multi Sensor Fusion Eren Allak1 Roland Jung2and Stephan Weiss1 Abstract Delay compensation in fi lter based sensor fusion frameworks for multiple sensors with varying delays and differ ent rates quickly results in large computational overhead should the delayed measurements be incorporated in a statistically meaningful way Even more so if high rate propagation sensors e g IMU are used This work presents an approach to implement such frameworks with signifi cant complexity reduc tion compared to standard implementations We set particular focus on the state covariance propagation as this chain of re computations i e FPFT Q per propagation step upon a delayed update is the dominant bottleneck We draw our inspiration from the scattering theory and propose a method which projects the idea of wave propagation to an effi cient concatenation of covariance propagation steps between fi lter updates Through this approach we reach a speed up of more than a factor of 10 for the covariance propagation and render the computational complexity independent of the number of propagation steps between fi lter updates We evaluated our method in simulation and with real data I INTRODUCTION Sensor fusion of multiple sensors leads to higher accu racy and robustness for system state estimation while the increased complexity poses problems that still need to be solved effi ciently Especially when the sensors provide mea surements at different rates and possibly with considerable delays the problem of multi delay multi sensor fusion be comes a challenge on computationally constrained platforms Some single sensor fusion frameworks tackle delayed sensor measurements with buffering techniques and recalculating all estimation steps once a delayed measurement arrives as shown in Fig 1 However the recalculations lead to spikes in the computation times and therefore performance degradation or failure for realtime systems in the worst case Especially the propagation of the covariance represented by Pprop FPFT Q is the largest computational burden while the matrix inversions in updates of low dimensional measurements and mean propagation through mean pre integration 1 2 can be done more effi ciently 1Eren Allak and Stephan Weiss are with the Department of Smart Systems Technologies in the Control of Networked Systems Group Universit atKlagenfurt 9020Klagenfurt Austria eren allak stephan weiss ieee org 2Roland Jung is with the Karl Popper School on Networked Autonomous Aerial Vehicles Alpen Adria Universit at Klagenfurt 9020 Klagenfurt Aus triaroland jung aau at The research leading to these results has received funding from the Austrian Ministry for Transport Innovation and Technology BMVIT under grant agreement n 855777 MODULES the ARL within the BAA W911NF 12 R 0011 under grant agreement W911NF 16 2 0112 and the Universit at Klagenfurt within the Karl Popper School on Networked Autonomous Aerial Vehicles Fig 1 A delayed measurement zd m arrives and needs to be processed The standard approach to handle delayed measurements is to recalculate all measurements which is computationally expensive and not feasible for multi sensor fusion with delays on resource constrained systems This work proposes an approach for multi sensor fusion considering multiple delays and multi rates by utilizing the scattering theory framework for effi cient covariance compu tations It is an effort to bring us one step further towards realtime delay compensated multi rate multi sensor fusion on computationally constrained platforms Also by allowing higher delays without negative effects of computation time spikes more computation time can be used to extract more accurate information from raw data of multiple sources providing further possibilities of improvement Multi and single sensor fusion can be posed as linear least squares estimation problems and solved by using state space models and recursive fi ltering A remarkable analogy with scattering theory leads to another approach to tackle these problems 3 being more effi cient for recalculating measurements and changing initial conditions e g upon delayed measurements Originally scattering theory or transmission line theory is used to describe the propagation of waves through media as for example sunlight scattered at a waterfall to form a rainbow or electro magnetic waves propagating through transmission lines The scattering theory was fi rst used in physics 4 and only two decades later in state estimation Interestingly its advantageous covariance computations were recently valued in path planning 5 6 3 decades later Now we extend its benefi ts to covariance pre integration Verghese et al 7 discovered Hamiltonian equations for smoothed estimators as the natural starting point for the development of a scattering theory framework for least squares estimation From this point of view states can be interpreted as waves going through a medium with layers which would be formed by parameters of the measurement and process equations In this work we will show how we can use this theory to construct a framework for effi cient covariance computations for the use in multi rate and multi 2019 IEEE RSJ International Conference on Intelligent Robots and Systems IROS Macau China November 4 8 2019 978 1 7281 4003 2 19 31 00 2019 IEEE6642 delay multi sensor fusion frameworks The paper is organized as follows Section II describes the related work Section III presents the scattering theory and our algorithm Section IV describes an example imple mentation with pose measurements Section V shows the experimental results and Section VII presents the conclusion II RELATED WORK Performing state propagation with sensors at high rates is benefi cial for state estimation as they provide rich informa tion in very dynamic situations Complexity reduction for state estimation using such high rate sensors in general is a well researched topic in the community Efforts are made to reduce the complexity through leveraging sparse matrix structures in non linear optimization frameworks 8 9 among others While non linear optimization approaches can inherently tackle multi rate and multi delay sensor read ings 10 they come at a much higher computational cost than fi lter based approaches despite making use of sparse structures Even on modern Systems on Chip SoCs it is challenging to achieve real time performance of such approaches for aggressive maneuvers of mobile platforms In a similar direction apart of using sparse matrix struc ture Forster et al 2 used pre integration of inertial mea surements introduced earlier by 1 between keyframes into relative motion constraints in a non linear optimization framework 11 to tackle the problem of growing number of optimization variables in real time nonlinear optimization for state estimation Complexity reduction in fi lter based approaches has been studied mainly for information fi lters 12 mainly leveraging sparse matrix structures or in simplifi ed forms for pre integration and subsequent observability analysis 13 All the above work did not propose a generally valid approach for fast covariance pre integration which is independent on the matrix structure Fast propagation of the covariance was done by 14 but only for the computation of the cross covariances of the current state with past states The cross covariances were propagated with accumulated state transition matrices F QN j iFj The change of initial condition as it occurs upon a delayed measurement cannot be done with this approach The two approaches in 15 and 16 for complexity reduction in multi sensor frameworks can only be used if the delayed time of arrival of a delayed measurement is exactly known in advance e g sensors with fi xed rate and fi xed delay Such assumptions usually do not hold for multi rate navigation sensors on mobile systems e g camera lidar Effi cient delay compensation was proposed by means of a dual fi lter approach by Larsen et al 15 The idea is to separate the covariance and the state computations for delayed updates by starting a second fi lter in parallel that includes the delayed measurement This approach is not scalable as every additional sensor replicates the entire fi lter Another way of delay compensation was presented by Van Der Merwe 16 using augmented states to include the past state and keep track of the cross covariances from the past state to the present state These cross covariances are then used to optimally fuse a delayed measurement at the current time Similar to 15 the increased computational complexity renders this approach not tractable for multi delay sensor fusion due to one complete state vector augmented per delayed measurement Our approach is inspired by the scattering theory frame work introduced in 17 3 and the theory behind it is described in Section III Prentice et al 5 made use of this aspect for effi cient path planning strategies considering uncertainty For their approach several simplifi cations could be applied which do not hold for real time system state estimation III SCATTERING THEORY FOR ESTIMATION Techniques of single sensor fusion can be employed to tackle the problem of multiple delays by recalculation as it is shown in Fig 1 In single sensor fusion usually a propagation sensor at high rates is used together with a single update sensor at a lower rate e g an IMU at 200Hz together with a loosely coupled visual SLAM Sensor at 20Hz 18 In this setting delayed measurements can be processed by naive recalculation But this technique is not suited to handle high rates of the propagation sensor together with multiple delayed sensors which exactly describes the problem of multi delay compensation in multi sensor fusion In this section we introduce the scattering theory frame work for state estimation and the scattering matrices used to compute covariances effi ciently over multiple measurements despite multiple potentially delayed sensor signals In anal ogy to the work on IMU pre integration 2 for compressing the expected propagation of several IMU readings in non linear optimization frameworks we denote our contribution as a more versatile covariance pre integration for fi lter based multi sensor estimators The importance of covariance recalculation as the main computational load is shown in section V A Linearized State Space Model The star product can be applied for estimation problems which may be described by a linear time varying state space model xi 1 Fixi Biui Giwi 1 yi Hixi vi 2 Using the notation Fi F ti at a discrete time tifor brevity With wi vi being uncorrelated white noise ran dom variables with variances Qi 0 Ri 0 and also not correlated with initial state x0with its variance P0 The process and the measurement equation are usually defi ned in a non linear form xi 1 f xi ui wi yi h xi vi 3 In the following we also defi ne zias sensor readings i e measurements These can propagate the state zt i ui e g IMU readings or update it zm i yi e g visual cues 6643 Delayed measurements are defi ned with a superscript zd To compute the propagation and update of fi lter estimates and the corresponding covariances f xi ui wi and h xi vi need to be linearized around the best estimate xifor the standard recalculation By linearizing we get the state space matrices F G B Q H R as described by Eq 1 and 2 B Scattering Theory This section introduces the fi ltering problem in the scat tering framework and builds the theoretical foundation of the effi cient covariance computations for delayed measurements zdas shown with the dark triangle in Fig 1 Consider a medium with fi xed length layers and some entities u and u propagating through this medium and assume that they obey a linear relationship relating emergent quantities and incident quantities The scattering matrix S is relating these quantities u S u The star product is used to combine adjacent sections of the medium into one layer described by one scattering matrix A multiplication on the right left is used to add a layer on the right left The star product is formally defi ned as in Eq 4 The variables inside of Eq 4 are the transmission coeffi cients and refl ection coeffi cients in the original defi nition of the star product 4 3 For state estimation Eq 12 explains the identities further below S a b cd A B CD A I bC 1aB Ab I Cb 1D c dC I bC 1ad I Cb 1D 4 By studying smoothed estimators 7 3 Hamiltonian equa tions similar to the scattering theory can be found The smoothed state and the adjoint variable can be interpreted as waves traveling in opposite directions in a scattering model in layers defi ned by the parameters F G B Q H R of the state space model The generator Mi i 1for a smoothed estimator is then Mi i 1 Fi 1Gi 1Qi 1GT i 1 HT i R 1 i HiFT i 1 5 I0 HT i R 1 i HiI F i 1 Gi 1Qi 1GT i 1 0FT i 1 Mm i Mt i 1 6 This generator has a simple factorization that corresponds to the well known pair of fi lter equations for update and propagation As it is seen in 6 the generator for an update has the form of Mmand a generator for propagation has the form of Mt These generators can be used to perform propagations Eq 8 or updates Eq 9 in a regular and well known E KF setup Only the covariance to be used needs to be formulated as a scattering matrix as in Eq 7 The entries represented by a dot are not used in this case since only the resulting covariance is required P I P 0I 7 Pprop P Mt 8 FPFT Q Pup P Mm 9 I KH P K PHT HPHT R 1 Taking this thought further scattering matrices S0combining many measurements zi zNas layers can be computed with zero or no initial conditions see 10 Scattering matrices that refer to a fi ltering problem with initial con ditions Pi covariance to state xi can be obtained by the star multiplication with the generator for initial covariance 7 as shown in 11 This can be seen as adding a boundary layer to the medium S0 i N Mi Mi 1 MN 10 Si N S0 i N 11 i NPi N Oi N T i N 12 To complete the analogy the entries of the scattering matrix Si Ncan be related to variables of a Kalman Filtering problem with the initial covariance Piand the measurements zi zN The forward transmission operator i Nis the closed loop transition matrix of all updates and propagations And the left refl ection operator Oi Nis the observability gramian associated with these measurements The most im portant entry for this work is the right refl ection operator Pi N being the covariance after applying all measurements to the initial covariance Pi Note that in this work we only focus on the effi cient covariance recalculation as we assume a multi sensor fusion framework with a large state vector compared to the dimen sions of the updates of the individual sensors As mentioned this puts the bottleneck of the estimator to the covariance recalculation i e the operation FPFT as shown in Sec V C Effi cient Covariance Computation In this section and the following section III D we develop the ideas for the so called Star Product based EKF SP EKF that uses the star product to effi ciently propagate covariances during recalculation of delayed measurements If the buffered measurements see Fig 1 would be pro cessed in order by the use of Eq 8 and Eq 9 the star product of the generators of the delayed measurement zd i and the propagation measurement ztwould already have a dense structure leading to many matrix inversions on the star product Eq 4 of remaining measurements Therefore depending on the order of computations the computational complexity can be considerably reduced as it is shown in this section The star product between two dense scattering 6644 matrices has two matrix inversions 14 matrix multiplications and 6 summations of matrix size Nx Nx which is the state size see Eq 4 The star product of Mtfor propagations keeps the same structure and only requires 4 matrix multi plications of size Nx Nx see Eq 13 No computations are needed for the star product of the generators of an update and propagation measurement Mmand Mt respectively the lower left part of Mmis just copied over to Mt See the decomposition 6 F i j Qi j 0FT i j F i Qi 0FT i F j Qj 0FT j 13 Thus we do not use the star product in chronological order instead all propagation measurements zt i 1 zt N are combined to one scattering matrix fi rst St i 1 N then star produce the preceding update measurement zm i with its generator Mm ion the left hand side The scattering matrix St i 1 N is called F chain as it concatenates i e pre integrates several propagation measurements By multiplying with the initial covariance generator i e adding a boundary layer see Eq 7 we get the fi nal result Furthermore not all four entries of the scattering matrix Si N need to be computed to obtain the covariance at the end only the Pi Nelement needs to be computed For the case of N propagation measurements one update and one initial condition as it is shown below N 1 star products are needed But with the computation order above only one inversion 4 N 4 matrix multiplications and 2 N summations are needed Si N Mm i Mt i 1 Mt i 2 Mt i N Si N Mm i St i 1 N Si N Smt i N Si N Pi N Smt i N Pi N Covariancerecalculationinthestandardrecalcula tion method requires for every propagation measurement zt i 1 zt Nre linearization and then propagation of the covariance with Pnew FPFT Q By using the F chain St i 1 N we recalculate this covariance for all measurements with just one operation In the case that the dimension of the measurement Nm is smaller than the state dimension Nx it is more effi cient to apply the measurement zmwith a ordinary Kalman Filter update because this involves just a matrix inversion of size Nm not Nx So instead of using Smt i N P is updated to Pup then Eq 7 is used to obtain upand up St i 1 N is calculated obtaining the same Pi N with less computations D Star Chain Implementation with Delayed Measurements In Fig 2 the recalculation of the covariance is shown under the dashed line at the bottom The upper box depicts the standard a