Distributed Data Fusion with Kalman Filters. Simon Julier Computer Science Department University College London

Transcription

1 Distributed Data Fusion with Kalman Filters Simon Julier Computer Science Department University College London

2 Structure of Talk Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 2

3 Motivation Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 3

4 Wilderness Search and Rescue 4

5 Wilderness Search and Rescue The priority is search which is fast and safe You have to find somebody before you can rescue them Time is often of the essence Safety of searchers must be ensured UAVs are an ideal tool to use: Rapidly collect data from wide area Can go where it s dangerous Automated platforms can operate even faster 5

6 UAV Search Task Fly around the environment Control based on prior distribution of target location Detect potentially interesting objects Localise potentially interesting objects Identify potentially interesting objects 6

7 Autonomous Platforms 7

8 Raw Sensor Data Returned 8

9 Target Localisation Estimate the state of a target in an area of interest using multiple UAVs UCL March

10 Tracking with Multiple UAVs 10

11 Goals of Lecture What causes the algorithms to fail so badly? How can we solve these problems optimally, and what constraints do they place on the solution? How can we solve the problem suboptimally, and what constraints do they pose on the problem? 11

12 Kalman Filters Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 12

13 System Description Let the state of the system at time step i be given by the state space vector Both the process and observation models are linear and are of the form: 13

14 Structure of the State Estimate The estimate consists of the tuple: where: is the mean vector; really a single numerical estimate a bit like MAP is the covariance ; really measure of mean squared error 14

15 Valid Estimates We need to have a criteria which says whether our system works or not Since we only have a state vector and a covariance matrix, our criteria of success is that our state is covariance consistent 15

16 Covariance Consistency We say that an estimate is conservative if it overestimates the actual mean squared error in the estimate, where 16

17 Matched 17

18 Conservative 18

19 Consistent 19

20 Inconsistent 20

21 Biasedness Authors often stipulate that the error needs to be zero-mean However, this is overly restrictive Any system with modelling errors cannot be consistent under this definition Since all models of the real world are wrong, no filter can be consistent 21

22 Biasedness Suppose Then 22

23 Biasedness Therefore, for the estimate to be consistent, 23

24 Matched Biased 24

25 Non-Gaussian Noise Models The definition similarly applies if the distribution is non-gaussian In this case, we just compute the moments of the distribution 25

26 Non-Gaussian Noise Models 26

27 Comparison with Entropy We could argue that instead of covariance consistency, we could use a measure like entropy However: To compute entropy we have to assume a distribution (e.g., Gaussian) Even with this assumption, it won t tell us if our estimate is too small 27

28 Entropy-Matched Distributions 28

29 First Iteration of the Kalman Filter Cycle Initialise Predict Update Observation 29

30 Prediction Step Under the assumption that the errors are small, linearised prediction equations are used 30

31 Kalman Filter Update Step We (arbitrarily) decide a linear update rule of the form where is the innovation vector

32 Kalman Filter Update Step The weight matrix is chosen to minimise the trace of the updated covariance matrix It can be shown that this is and this gives a covariance update equation of the form

33 Debiased Coordinate Conversions A sensor measures the range r and bearing to a target The aim is to estimate the (x, y) coordinates of the target The measurement model is, of course: 33

34 Debiased Coordinate Conversions 34

35 Debiased Coordinate Conversions in Action 35

36 [Mapping Example]

37 Extended Kalman Filter A common assumption is that the errors are small Therefore, the first two moments are approximated as Various kinds of analytical and numerical moment approximations are widely used as well

38 Kalman Filters Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 38

39 Distributed Fusion Kalman Filter Cycle Initialise Predict Update Local Observation Broadcast Estimate Update Remote Estimate Observation Other Nodes Additional steps due to distribution 39

40 Basic Idea of DDF Each platform maintains its own estimate of the target state, Each node runs a Kalman filter locally and fuses locally taken measurements The update is distributed to other nodes which fuse with it 40

41 However, There Is A Slight Complication The state information stored in each node is not independent of the information in other nodes Common process noise Occurs whether or not nodes have exchanged information Common measurement history Occurs when nodes exchange information Assuming that state estimates are independent of one another is bad However, often used in so-called weak coupling of, say, GPS and INS systems 41

42 Dependent Information and Information Sets Each node collects its own set of data, which is independent of the other node

43 Fusion of Information Sets Estimates (information sets) exchanged between nodes

44 Fusion of Independent Information Sets

45 When Do Independent Sets Arise? Independent sets arise when the information set to each node is conditionally independent This can only happen if you can guarantee: The target is stationary The poses of the UAVs are known perfectly The same observation information is only ever used once

46 Multiple Platform Fusion

47 Dependent Information Sets Both sets now contain common information; not conditionally independent

48 Fusion of Dependent Information Sets New information Common information New information

49 Assuming Conditional Independence Double counted term

50 Double Counting in State Space Form Within a Kalman filter, the dependency is through the values of the cross correlations These can be evaluated by considering the state of the entire network, including the joint state of all platforms, all objects being tracked, etc. 50

51 Double Counting in State Space Form The full covariance structure of this estimate is

52 Double Counting in State Space Form However, if we only maintain the marginals for each platform separately,

53 Double Counting in State Space Form The error in the approximate covariance matrix is Therefore, we are using an inconsistent approximation of our network and our system will fail 53

54 Assuming State Estimates Are Independent

55 Optimal Distributed Data Fusion Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 55

56 The Right Way to Solve the Problem Chong and Mori showed that this can be implemented with a modified form of Bayes Rule, Cancel out common information This cancels out the common information between nodes This can only be computed locally with special network topologies

57 Approach 1: Distribute Observations Broadcast all observations to all nodes 57

58 Pros and Cons Advantages: Each node has optimal estimate for all time Distribution provides no additional complexity to fusion algorithm Actually used in practice Disadvantages: Requires all nodes to have the same communication and computational abilities Requires extremely large bandwidth Introduces implicit assumption that all nodes have exactly the same estimate 58

59 Approach 2: Fully-Connected Network Broadcast all updated state estimates to all nodes 59

60 Fully-Connected Networks The easiest way to implement a fully connected network is to use the inverse covariance (or information) form of the Kalman Filter The state space is replaced by the information variables 60

61 Updating in Information Form Using information form, the update simplifies to where the information from the observations is 61

62 Distributed Information Updates Since the information from the observations is independent of the state, i n and I n are independent of previous state estimates and can be safely distributed The update rule simply becomes 62

63 Fully-Connected Network Advantages: Each node has optimal estimate for all time Broadcasting the observation information variables potentially saves bandwidth Disadvantages: Requires all nodes to have the same communication and computational abilities Still requires O(N 2 ) communication links Introduces explicit assumption that all nodes have exactly the same estimate (important if linearising e.g., with an EKF) 63

64 Approach 3: Hierarchical Network Network has master and slave nodes Slaves fuse data locally Estimates sent to master which fuses them together Revised estimate broadcast back to slaves 64

65 Fusion in the Slave The slave updates using the information Kalman filter equations: 65

66 Fusion in the Master The master updates by summing the information from all the slaves To compensate for the prediction which was sent out, the master must subtract out common information, 66

67 Hierarchical Network Advantages: Each node has optimal estimate for all time The number of communication links is O(N) Disadvantages: Additional latency One node is privileged; failure of that node causes the whole network to fail 67

68 Approach 4: Channel Filters Constrain the network to be a tree Single path between any pair of nodes Use channel filters to subtract off common information 68

69 Estimating Common Information Consider a link between a pair of nodes i and j The channel filter maintains common information across the link It has its own information estimate, 69

70 Updating Local Nodes The Channel Filter is a regular Kalman Filter but works with the information exchanged between i and j rather than the observation data directly First, let the update at filter i using the local sensor observations be written as 70

71 Fusing With Nearby Nodes The updated estimate is given by summing all the independent information from a node s neighbours, 71

72 Updating the Channel Filters The channel filter update is given by recursively updating with the difference in information variables from the two nodes, 72

73 Channel Filters in Action 73

74 Advantages and Disadvantages Advantages: The number of communication links is O(N) Optimal in a time-delayed sense Disadvantages: Estimates at all nodes differ Single path of communication; no redundancy If the network is reconfigured, the channel filters have to be recalculated from scratch Global time synchronisation 74

75 Hybrid Architectures Channel filters can be mixed-and-matched with other local topologies like observation distribution or master-slave 75

76 Review of Techniques So Far It is possible to develop optimal algorithms for distributed data fusion using local message passing only However, these techniques rely on special network topologies: Fully connected Tree-connected In general, preserving these topologies can be difficult and undesirable 76

77 Adhoc Network Arbitrary network with loops and cycles Complete flexibility and redundancy 77

78 Distributed Data Fusion in Adhoc Networks However, it has been shown that no local data fusion scheme can be used to develop consistent, optimal estimates in this situation Therefore, it appears that DDF is strongly limited to the case of very particular data fusion architecture Alternative approach: can we develop mathematically rigorous suboptimal solutions? 78

79 Suboptimal Distributed Data Fusion Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 79

80 Double Counting in State Space Form Recall again that the problem is that we want to know the full joint covariance However, we only know the marginalised form 80

81 Double Counting in State Space Form From knowledge of the marginals alone, it is not possible to reconstruct the full joint covariance matrix However, because the joint covariance matrix must be positive semidefinite, we know there are constraints on what the cross correlations look like Therefore, we can exploit these constraints to develop update rules which are consistent for any feasible cross correlation 81

82 The Kalman Filter with Correlated Noise First consider the case that the observation noise is not independent of the filter state, It can be shown that the expectations become

83 Properties of Updated Covariances 83

84 Applying the Results to The update which generates a family of ellipses which circumscribe the intersection region is given by This is the same as a Kalman filter update, but with 84

85 Covariance Intersection 85

86 Choosing The free parameter is used to trade-off between the prediction and the observation It should be chosen to minimise some measure of uncertainty in the estimate Trace Determinant (better) The optimisation is convex and so many simple solver algorithms can be used Some closed form solutions have been developed as well 86

87 Covariance Intersection In Action 87

88 Probabilistic Interpretation of WGMs Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 88

89 Probabilistic Interpretation Great, so this works with means and covariances - but is it actually doing something valid from a probability distribution point of view? It turns out, that a generalisation of CI is equivalent to computing the weighted geometric mean,

90 WGM Does Not Double Count Single counted term

91 Structure of the Fusion Rule The fusion rule has the form where Function of new information Common information single-counted

92 Information Losses and Gains Therefore, we now need to ask what is the effect of We can assess this in several ways: By observation Pointwise bounds Information measures Surprisingly hard; still a work in progress

93 Example Distributions

94 Effect of

95 Effect of

96 Effect of

97 Effect of

98 Effect of

99 Effect of

100 Pointwise Bounds It is possible to establish pointwise bounds which apply at each point in the distribution Although pointwise bounds play no special role in Bayesian statistics, they provide some insight into the behaviour of the fusion rule

101 Bounds for the Unnormalised Distribution Let This is always squeezed between the distributions,

102 Illustration of the Unnormalised Bound

103 Lower Bound Consider the distribution where The WGM obeys the lower bound

104 Illustration of the Lower Bound

105 Interpreting the Lower Bound The minimum value of a distribution plays no special role in Bayesian statistics However, the bound from below Avoids degenerate cases The support has to contain the intersection of the supports of the prior distributions Lower bounds on distributions often play a role in practical filtering algorithms Truncate distributions or modes in MHT if the probability is too small

106 Upper Inequality There can exist an x such that The fact that the distribution can exceed the maximum suggests that fusion can occur The distribution becomes more concentrated

107 Illustration of the Upper Inequality

108 Updated Distribution

109 Summary Motivation Kalman Filters Double Counting Optimal Distributed Data Fusion Suboptimal Distributed Data Fusion Probabilistic Interpretation of WGMs Summary 109

110 Summary Distributed data fusion is important for many applications However, estimates are not conditionally independent Optimal solutions can be used in just limited circumstances Suboptimal algorithms can be used more widely The KF is more than Bayes with Gaussians! 110