Incorporation of Time Delayed Measurements in a. Discrete-time Kalman Filter. Thomas Dall Larsen, Nils A. Andersen & Ole Ravn

Transcription

1 Incorporation of Time Delayed Measurements in a Discrete-time Kalman Filter Thomas Dall Larsen, Nils A. Andersen & Ole Ravn Department of Automation, Technical University of Denmark Building 326, DK-2800 Lyngby, Denmark ftdl, naa, org@iau.dtu.dk Niels Kjlstad Poulsen Department of Mathematical Modelling, Technical University of Denmark Building 321, DK-2800 Lyngby, Denmark nkp@imm.dtu.dk Abstract In many practical systems there is a delay in some of the sensor devices, for instance vision measurements that may have a long processing time. How to fuse these measurements in a Kalman lter is not a trivial problem if the computational delay is critical. Depending on how much time there is at hand, the designer has to make trade os between optimality and computational burden of the lter. In this paper various methods in the literature along with a new method proposed by the authors will be presented and compared. The new method is based on \extrapolating" the measurement to present time using past and present estimates of the Kalman lter and calculating an optimal gain for this extrapolated measurement. 1 Introduction This paper considers the problem of designing discretetime Kalman lters to systems where some results of the measurements are delayed. Most of the work that has been done prior in this eld considers only lters where no measurements has been fused in the delay period (i.e. the time period from the measurement is taken till it is available), see for instance [1]. In many applications, especially in the eld of autonomous vehicles, however, the Kalman lter will fuse measurements from faster sensors in this delay period. Typically the delayed measurements will origin from a vision system and the fast measurements from a sonar or dead reckoning system. Other authors have focused on systems, where system and output equations have a common delay, as in [2] and in [3]. Kalman lters where the output is delayed only a fraction of the sample time can be handled in an optimal manner by modifying the output equation of the lter as shown in [4]. Delays consisting of a small number of samples can be handled optimally in the discrete-time Kalman lter by augmenting the state vector accordingly - see for instance [5] or [6]. As the system order increases with the delay size, however, this method is mostly used when the time delay is a small number of samples, as the computational burden otherwise may become undesirably high. If only a few measurements are fused in the delay period or if the computational burden of the lter is uncritical, an optimal lter estimate incorporating the delayed measurement can be obtained simply by recalculating the lter through the delay period. As this often becomes too time consuming in practical systems, a method is derived in [7] where it suces to calculate a correction term and add this to the lter estimate when the delayed measurement arrives. This method is optimal in certain time intervals under certain conditions and will be described in this paper along with a modication introduced by the authors that extends these periods of optimality. Furthermore, a new method that does not guarantee optimality under all conditions but is useful in many practical systems will be described. The new method, based on extrapolating the delayed measurements, is especially suited for the type of ltering problems mentioned above, where a number of (imprecise) measurements has been fused in the delay period. Regardless of the number and nature of the measurements this method is a simple and computationally cheap way of accounting for delays.

2 2 System and Filter Equations A linear discrete system observed by non-delayed measurements where both process and measurements are inuenced by additive Gaussian noise can be put in state space form as follows: x k+1 = A k x k + B k u k + w k ; (1) z k = C k x k + v k ; (2) where: w k N(0; Q k ) and v k N(0; R k ). Without loss of generality we can assume that the two noise sources v k and w k are independent (E w i vj T = 0). The optimal state estimator minimizing the variances of the estimation error will then be a Kalman lter. A proof for this, along with a derivation of the Kalman lter equations can be found in [8]. The equations are summarized below in (3) to (7): ^x k+1 = A k^x k (+) + B k u k (3) P k+1 = A k P k (+)A T k + Q k (4) Ck P k C T k + R k?1 (5) K k = P k Ck T ^x k (+) = ^x k + K k [z k? C k^x k ] (6) P k (+) = [I? K k C k ] P k (7) If the system (1) furthermore has an output that is delayed N samples, for instance due to a slow sensor or a long processing time of the sensor data, there will be a second output equation: z k = C s x s + v k ; v k N(0; R k); (8) where s = k? N. x s ^x s z s z k x k z k ^x k System state Filter state Figure 1: System with an N sample delayed output. The delayed measurement cannot be fused using the normal Kalman lter equations but requires some modications in the structure of the lter. 3 Incorporating Delayed Measurements As mentioned in section 1 a number of dierent methods has been proposed for incorporating delayed measurements in the Kalman lter. The system dened in section 2, however, has to the authors' knowledge only been treated in [7]. The method proposed by Alexander will in some cases be highly suited to the type of systems considered in this paper and will therefore be summarized in this section along with a modication suggested by the authors. Subsequently, a new method which can be used when Alexander's method comes to short, will be presented. 3.1 Updating Covariance and State at Dierent Times Using the standard Kalman lter equations, the measurement zk should be fused at time s, causing a correction in the state estimate and a decrease in the state covariance. As the state covariance matrix decides the Kalman gain, the measurements occurring after this will all be fused dierently than if the measurement update for zk is omitted. If therefore the measurement zk is delayed N samples and fused at time k, the data update should reect the fact that the N data updates from time s to k, and therefore the state and covariance estimates, have all been aected by the delay in a complex manner. Equations that account for this when fusing zk at time k has been derived in [7] but are of such complexity that they in many cases are not feasible 1. It is therefore suggested that if the measurement sensitivity matrix, Cs, and the noise distribution matrix, R k, is known at time s, the lter covariance matrix should be updated as if the measurement is available. This leads the measurements in the delay period to be fused as if zk had been fused at time s. At time k, when zk is available, incorporating zk is then greatly simplied, by adding the following quantity after z k has been fused: ^x k = M K s (z k? C s ^x s) (9) If the delay is zero, M is the identity matrix. For N > 0, M is given by: (I? K 0 k?i C k?i)a k?i?1 (10) The prime on K 0 signies that these Kalman gain matrices have been calculated using a covariance matrix updated at time s with the covariance of the delayed measurement. As one factor in the product above can be calculated at each sample time the method only requires two matrix multiplications at each sample time. The method implies that the covariance of the lter will be wrong in a period of N samples leading measurements in this period to be fused suboptimally. However, after the correction term in (9) is added, the lter state and covariance will once again be optimal. 1 In fact the computational complexity of these equations is comparable to recalculating the Kalman lter through the delay of N samples.

3 3.1.1 A Modication to Ensure Optimality: As mentioned in section 3.1 the lter estimates will be suboptimal from the time the measurement zk is taken till it is fused. This can cause problems, especially if data validation is performed using the incorrect P - matrix leading the lter to discard valid measurements. At the expense of some additional computations this problem can be avoided by making the lter estimate optimal at all times. This is done by starting up an extra lter at time s that uses the covariance R k as suggested by [7] and running this in parallel to the optimal lter. Up until time k, the estimates from the optimal lter will be used but at time k when the delayed measurement arrives, this will be fused in the parallel lter and this lter will now have the optimal state estimate. This simple modication of the method guarantees optimality at all times but imposes twice the computational burden in the delay time period (from s to k). 3.2 Extrapolating the Measurement The method described in section 3.1 required the measurement sensitivity matrix, Cs, and the noise distribution matrix, R k, to be known at time s. In many cases they are not. If for instance the measurement is a vision measurement, the uncertainty of the measurement will often be unknown until the data is processed as it depends on the relative positioning of the camera and object. Similarly Cs may depend on positioning and occlusions and therefore also not be known until the data is processed. A method that does not require knowledge about zk until time k is therefore needed. For non-delayed measurements, the residual used to calculate the new estimate, is dened by: k = z k? C k^x k (11) When the N-samples delayed measurement given in (8) arrives at time k, the lter and measurement state will relate to dierent times and a residual relating to time k cannot be attained. But if the lter state from time s has been stored, the residual that would have been used at time s if the measurement had not been delayed can be used in the update at time k: k := s = z k? C s ^x s (12) This is equivalent of extrapolating the measurement,, to a present measurement, zint z k k : z int k = z k + C k ^x k? C s ^x s ; (13) and fusing this at time k using the ordinary residual dened in (11). The extrapolated measurement is given by: z int k = z k + C k ^x k? C s ^x s = Cs x s + vk + C k ^x k? Cs ^x s = Ck x k + vk + Ck ~x k? Cs ~x s (14) = Ck x k + vk int (15) where the estimation error ~x = ^x? x. This new extrapolated measurement is seen to have the standard form as in (2), except that here there is a correlation between the noise process vk int and the state x k. The optimal gain for fusing zk int and the resulting lter covariance decrease will now be derived. If the measurement is fused using the data update in (6) with an arbitrary gain K k, the estimation error, ~x k (+), becomes: ~x k (+) = (I? K k C k)~x k + K k v int k (16) The variance of the estimation error is: P k (+) = Ef~x k (+)~x T k (+)g = (I? K k C k )P k(i? K k C k )T +(I? K k Ck )Ef~x kvk intt gkk T +K k Efvk int ~x T k g(i? K k Ck) T +K k Efvk int vintt k gkk T (17) Let: M = Ef~x s ~x T k g. The covariances in (17) can then be found from (14): Ef~x k vk intt g = P k Ck T? M T Cs T (18) Efvk int vintt k g = R k + Ck P kck T + Cs P scs T?C s M CT k? C k M T C T s (19) Inserting (18) and (19) in (17) and rearranging the terms leads to: P k (+) = P k? M T Cs T KT k? K k Cs M +K k Cs P scs T KT k + K k R k KT k (20) The gain, K k, should be chosen so that the variances of the estimation errors are as low as possible. This is obtained k k = 0 Dierentiating (22) and isolating K k yields: K k = M T C T s C s P s C T s + R k?1 (21) This is therefore the optimal gain for fusing the measurement zk int. Inserting (21) in (20) leads to a simpler form for P k (+): P k (+) = P k? K k C s M (22) The covariance matrix, M, can be found by observing the propagation of the estimation error ~x. Through the time update (3), ~x becomes: ~x k+1 = A k ~x k (+)? w k

4 After the data update (6), ~x is: ~x k (+) = (I? K k C k )~x k + K k v k Through N succeeding time and data updates from time s to k, the estimation error therefore becomes: ~x k (+) = [(I? K k?i C k?i )A k?i?1 ] ~x s (+) +f 1 (w s w k?1 ) + f 2 (v s+1 v k ) Where f 1 and f 2 are functions of the noise sequences v and w. As ~x s is uncorrelated with these noise sequences, the covariance, M, becomes: where: M = Ef~x s ~x T k g = P s A T s+i(i? K s+i+1 C s+i+1 ) T = P s M T ; (23) (I? K k?i C k?i )A k?i?1 (24) Observe that the correction term, M, is similar to (10) in section 3.1 except that in (10) the Kalman gains reect the zk data update at time s. Substituting (23) in (21) and (22) yields: and: P k (+) = P k? K k C s P sm T (25) K k = M P s C T s C s P s C T s + R k?1 (26) Notice that when the delay N is zero, I and the Kalman gain equation (26) reduces to the standard form (5) and the covariance update (25) reduces to (7). So by calculating an extrapolated measurement, z int k, using (13) the delayed measurement can be fused in a simple and computationally cheap manner using equation (24) - (26) Optimality of Extrapolation: Observe that though the gain suggested in (26) is statistically optimal for fusing the extrapolated measurement, the extrapolation method itself is still not optimal. In order for the method to be optimal the Kalman gains in (24) should reect a data update at time s, that is equation (10) should equal (24). If no measurements has been fused in the delay period equation (24) and (10) are identical and the extrapolation will be optimal. This important case is quite common in praxis, for instance when a dead reckoning system is used as the system model. This is a popular Kalman lter type on mobile robots, see for instance [9] or [10]. Here: A k?i?1 The matrix A is a linearized system matrix as described in [10]. If the movement of the robot is assumed linear the output from the dead reckoning system can be transformed to a linear and angular displacement d k and k. The robot coordinates in a global coordinate frame can then be updated by (see [11]): X k+1 = X k + d k cos( k + k 2 ) k+1 = k + d k sin( k + k 2 ) k+1 = k + k The A matrix is seen to be: A = a a a 13 =?d sin( 2 ) a 23 = d cos( 2 ) Considering the structure of A, the matrix M can be found by: 2 4 P 1 0 a 13;i P a 23;i When this particular lter type is used, the extrapolation method therefore provides a very simple and optimal 2 way of accounting for delays. 4 Example Consider now, as a continuous plant a typical DC motor with the shaft position, (t), as output and the anchor voltage, u(t) as input. A potentiometer and a camera both observe the shaft position. The potentiometer measurement is continuous and noisy and the vision measurement is discrete, delayed but accurate. Both the process and the measurements are inuenced by independent Gaussian white noise processes. A state space formulation of the plant is given below: _x(t) = A c x(t) + B c u(t) + w(t) = 0 1 0?! m x(t) + z(t) = [1 0]x(t) + v(t) k m u(t) + w(t) z (kt ) = [1 0]x(kT? t d ) + v (kt ); k 2 N 2 Strictly speaking the lter is not optimal as the system is nonlinear.

5 The distribution of the process noise is: w(t) N(0; 0 ); Q and the distribution of the measurement noises are: v(t) N(0; R); v (kt ) N(0; R ) The plant is discretized and a discrete Kalman lter is designed as described in section 2. This plant is now simulated using dierent methods to account for the delayed measurement, namely by: A Recalculating the lter when z k arrives B Using Alexander's method C Using the modied version of Alexander's method D Extrapolating the measurement The variances of the potentiometer noise is R = 10?3 while the proces noise, Q, and the vision noise, R, are varied in the simulation. Table 1 shows the averaged variances of the estimation error, e = y?^y, from Monte Carlo simulations using the four methods above and using dierent values for the noise variances and the initial estimation error. The variances are normalized with the results from a simulation with no delay on the vision measurements. The Q R A B C D 0 10? ? ?4 10? ?4 10? Table 1: Normalized estimation error variance normalized variances in table 1 are all higher than one, meaning that no method fully compensates for the delay (so rather unsurprisingly we would prefer that the measurement was not delayed). Also it is observed that the modied Alexander as expected yields the same results as recalculation as both these methods are optimal. It is also seen that although Alexander's method is optimal in the time intervals outside the delay period and the extrapolation method here is suboptimal always, it is not obvious which of the two methods performs best. In the two simulations with low vision measurement noise the extrapolation yields the lowest variance and in the other two simulations Alexander's method performs best. It is clear that comparisons between these two methods should be done with some caution as the relative performance of the methods changes in dierent conditions. In comparing the lters the computational burden imposed by the lters should also be considered. Table 2 shows the amount of oating point operations in the dierent lters, scaled with respect to a lter that fuses an undelayed vision measurement at time k. Though these numbers are illustrative, of course the absolute as well as the relative size strongly depends on the specic system, especially the system order. Sample A B C D s s! k k N Table 2: Normalized computational burden It is obvious that recalculation can only be used if the delay N is small or if the computation time is uncritical. If the measurement variance and sensitivity matrix are known when the measurement is taken the modied Alexander yields exactly the same results with less computation. Both the extrapolation method and Alexander's method are even cheaper computationally but does not guarantee optimality at all times. 5 Conclusion In this paper a new method for incorporating measurements with delays of arbitrary size in a Kalman lter has been introduced. The method is fast and can be applied to a wide variety of systems, but does not guarantee optimality under all conditions. If the covariance and the measurement sensitivity matrix of the delayed measurement is at hand at the time where the measurement is taken, a dierent method introduced in [7] and modied in this paper will give an optimal and fairly fast estimate. It was shown that if no measurements are fused in the delay period, the extrapolation method will be optimal. If an odometric lter type is used where for instance encoder readings are used as a system model, the extrapolation method will also be optimal and the algorithm very simple. References [1] Stelios C. A. Thomopoulos. Decentralized ltering in the presence of delays: Discrete-time and continuous-time case. Information Sciences, 1994.

6 [2] G. N. Mil'shtein and S. A. P'yanzin. A discretization method in designing an optimal lter for systems with delay. In Automation and Remote Control, [3] Sang Jeong Lee, Seok min Hong, and Graham C. Goodwin. Loop transfer recovery for linear systems with delays in the state and the output. International Journal of Control, [4] Gert L. Andersen, Anders C. Christensen, and Ole Ravn. Augmented models for improving vision control of a mobile robot. In 3rd IEEE Conference on Control Applications, [5] Feng-Hsiag Hsiao and Shing-Tai Pan. Robust kalman lter synthesis for uncertain multiple timedelay stochastic systems. Journal of Dynamic Systems, measurement, and control, [6] E. Kaszkurewicz and A. Bhaya. Discrete-time state estimation with two counters and measurement delay. In Proceedings of the 35th IEEE Conference on Decision and Control, [7] Harold L. Alexander. State estimation for distributed systems with sensing delay. In SPIE vol Data Structures and Target Classication, [8] Arthur Gelb. Applied Optimal Estimation. The analytical Sciences Corporation, [9] F. Chenavier and J. L. Crowley. Position estimation for a mobile robot using vision and odometry. In Proceedings of the 1992 IEEE International Conference on Robotics And Automation, Nice, France, [10] Satoshi Murata and Takeshi Hirose. Onboard locating system using real-time image processing for a self-navigating vehicle. IEEE Transactions on Industrial Electronics, 40(1), February [11] C. M. Wang. Location estimation and uncertainty analysis for mobile robots. In Proceedings of the 1988 International Conference on Robotics and Automation, 1988.